?? p_obs.m
字號:
function [obs_p,state_p]=p_obs(obs,state)
% function to place vehicle centered observations in
% global coordinates for checking of consistency
globals;
[emp,OSIZE]=size(obs);
[temp,SSIZE]=size(state);
if (SSIZE ~= OSIZE)
error('Unmatched state and observation dimensions')
end
obs_p=zeros(2,SSIZE);
state_p=zeros(2,SSIZE);
numobs=0;
for i=1:SSIZE
if obs(3,i) ~= 0
numobs=numobs+1;
obs_p(1,numobs)=state(1,i)+(obs(1,i).*cos(obs(2,i)+state(3,i)));
obs_p(2,numobs)=state(2,i)+(obs(1,i).*sin(obs(2,i)+state(3,i)));
state_p(1,numobs)=state(1,i);
state_p(2,numobs)=state(2,i);
end
end
obs_p=obs_p(:,1:numobs);
state_p=state_p(:,1:numobs);
?? 快捷鍵說明
復制代碼
Ctrl + C
搜索代碼
Ctrl + F
全屏模式
F11
切換主題
Ctrl + Shift + D
顯示快捷鍵
?
增大字號
Ctrl + =
減小字號
Ctrl + -