hfun.m

一个关于卫星导航方面的ekf滤波程序

function observ = hfun(state, Noi)
% EKF-Observation Function
% Input: Current state, Observation noise
% Output: New observation

xm=330688.741669;
ym=133414.811161;
zm=70496.488359;
navs =[ 0.1305    0.6823    0.7193   -0.7838   -0.5270    0.3286    0.4592   -0.8748    0.1542];% calculated by navstar.m, normalized
xs1=navs(1);
ys1=navs(2);
zs1=navs(3);
xs2=navs(4);
ys2=navs(5);
zs2=navs(6);
xs3=navs(7);
ys3=navs(8);
zs3=navs(9);

[row col]=size(state);
for k=1:col
    x=state(1,k);
    y=state(2,k);
    z=state(3,k);

    r=sqrt(x^2+y^2+z^2);
    rm=sqrt(xm^2+ym^2+zm^2);
    rsm=sqrt((x-xm)^2+(y-ym)^2+(z-zm)^2);
    
    obser(:,k)=[subangle(pi,acos((x*xs1+y*ys1+z*zs1)/r))
                subangle(pi,acos((x*xs2+y*ys2+z*zs2)/r))
                subangle(pi,acos((x*xs3+y*ys3+z*zs3)/r))
                subangle(pi,acos(((x-xm)*xs1+(y-ym)*ys1+(z-zm)*zs1)/rsm))
                subangle(pi,acos(((x-xm)*xs2+(y-ym)*ys2+(z-zm)*zs2)/rsm))
                subangle(pi,acos(((x-xm)*xs3+(y-ym)*ys3+(z-zm)*zs3)/rsm))];
            
  
end
if col==1
    obser(:,k) = obser(:,k) + [Noi(1) Noi(1) Noi(1) Noi(2) Noi(2) Noi(2)]';
    obser(1,k) = addangle(obser(1,k), Noi(1));
    obser(2,k) = addangle(obser(2,k), Noi(1));
    obser(3,k) = addangle(obser(3,k), Noi(1));
    obser(4,k) = addangle(obser(4,k), Noi(2));
    obser(5,k) = addangle(obser(5,k), Noi(2));
    obser(6,k) = addangle(obser(6,k), Noi(2));
  else
    obser = obser + Noi;%%%%%%!
  end
observ=obser(:,:);