memsobs.m

来自「Kalman filter that can be simulated unde」· M 代码 · 共 14 行

function y = memsobs(x,w)
% Prediction of observation
% 
% Check if variables should be initailized
global xb10, yb10, zb10, xb21, yb21, zb21, x10, y10, z10;

if nargin==1
   return
end
a = x(2); b = x(4); c = x(6);
R10=[cos(a)*cos(b),cos(a)*sin(b)*sin(c)-sin(a)*cos(c),cos(a)*sin(b)*cos(c)+sin(a)*sin(c);sin(a)*cos(b),sin(a)*sin(b)*sin(c)+cos(a)*cos(c),sin(a)*sin(b)*cos(c)-cos(a)*sin(c);-sin(b),cos(b)*sin(c),cos(b)*cos(c)];
y(1:3) = R10 * [0 0 9.8].';
y(4) = ((cos(a)*cos(b)*xb21+(cos(a)*sin(b)*sin(c)-sin(a)*cos(c))*yb21+(cos(a)*sin(b)*cos(c)+sin(a)*sin(c))*zb21+x10-xb10)^2+(sin(a)*cos(b)*xb21+(sin(a)*sin(b)*sin(c)+cos(a)*cos(c))*yb21+(sin(a)*sin(b)*cos(c)-cos(a)*sin(c))*zb21+y10-yb10)^2+(-sin(b)*xb21+cos(b)*sin(c)*yb21+cos(b)*cos(c)*zb21+z10-zb10)^2)^(1/2);
y = y + w;