ekf.m

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function [Xekf,Pout]=ekf(Xin,Z,Pin,Qekf,Rekf,Station)

T=0.5;
F=[1,0,0,T,0,0,T^2/2,0,0;
   0,1,0,0,T,0,0,T^2/2,0;
   0,0,1,0,0,T,0,0,T^2/2;
   0,0,0,1,0,0,T,0,0;
   0,0,0,0,1,0,0,T,0;
   0,0,0,0,0,1,0,0,T;
   0,0,0,0,0,0,1,0,0;
   0,0,0,0,0,0,0,1,0;
   0,0,0,0,0,0,0,0,1]; % 状态转移矩阵

% 对粒子的一步预测
[y1,y2,y3,y4,y5,y6,y7,y8,y9] = ffun(Xin);
Xpre = [y1,y2,y3,y4,y5,y6,y7,y8,y9]';

% 计算状态的雅可比矩阵，此处是一维，很简单，如果是线性的，那么直接是F，所以应该传递一个F参数进来，或者将F写在这里
Jx=F;
 
% 方差预测
Pekfpre = Qekf + Jx*Pin*Jx';
 
% 观测预测
[dd,alpha,beta] = hfun(Xpre,Station);
Zekfpre = [dd,alpha,beta]';
 
% 计算观测的雅可比矩阵
 % 求Jacobian矩阵H，H为3行9列的矩阵
D = Dist(Xpre,Station);
DD = Dist3(Xpre,Station);
Jy = [(Xpre(1,1)-Station(1,1))/DD,(Xpre(2,1)-Station(2,1))/DD,(Xpre(3,1)-Station(3,1))/DD,0,0,0,0,0,0;
      -(Xpre(2,1)-Station(2,1))/D^2,(Xpre(1,1)-Station(1,1))/D^2,0,0,0,0,0,0,0;
      (1/(1+((Xpre(3,1)-Station(3,1))/D)^2)).*(-2*(Xpre(1,1)-Station(1,1))/D^4),(1/(1+((Xpre(3,1)-Station(3,1))/D)^2)).*(-2*(Xpre(2,1)-Station(2,1))/D^4),(1/D)/(1/(1+((Xpre(3,1)-Station(3,1))/D)^2)),0,0,0,0,0,0];

% EKF方差更新
M = Rekf + Jy*Pekfpre*Jy';
 
% 计算Kalman增益
K = Pekfpre*Jy'*inv(M);
 
% EKF状态更新
% 这里就是EKF建议分布的均值，用它就可以指导粒子分布
Xekf=Xpre+K*(Z-Zekfpre);
 
% EKF方差
% 这里就是EKF建议分布的方差，用它就可以指导粒子网的“半径”
Pout = Pekfpre - K*Jy*Pekfpre;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%% 求两点部分距离（三维）Sx2 + Sy2 %%%%%%%%%%%%%%
function D = Dist(X1,X0)
D = sqrt( (X1(1,1)-X0(1,1))^2+(X1(2,1)-X0(2,1))^2 );

%%%%%%%%%%%%% 求两点距离（三维）%%%%%%%%%%%%%%
function DD = Dist3(X1,X0)
DD = sqrt( (X1(1,1)-X0(1,1))^2+(X1(2,1)-X0(2,1))^2+(X1(3,1)-X0(3,1))^2 );