扩展卡尔曼滤波（EKF）实现三维位置估计

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%  扩展Kalman滤波实现三维位置估计
%  观测有距离、俯仰角、偏航角
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function ekf_for_track_9
clc;
clear;
n=9;
T=0.5;        % 雷达扫描周期
N=50;         % 总的采样次数

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]; % 状态转移矩阵

Q=[1 0 0 0 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 0.01 0 0 0 0 0;
    0 0 0 0 0.01 0 0 0 0;
    0 0 0 0 0 0.01 0 0 0;
    0 0 0 0 0 0 0.0001 0 0;
    0 0 0 0 0 0 0 0.0001 0;
    0 0 0 0 0 0 0 0 0.0001];

% R=0.1*pi/180;   % 观测噪声方差（因为只有一个观测，所以是一个值，不是协方差矩阵）
R = [100 0 0;             % 观测噪声协方差矩阵  
    0 0.001^2 0
    0 0 0.001^2];

X=zeros(9,N);   % 目标真实位置、速度
X(:,1)=[1000;5000;200;10;50;10;2;-4;2]+sqrtm(Q)*randn(n,1);

Z=zeros(3,N);   % 传感器对位置的观测

x0=0;           
y0=0; 
z0=0;
Xstation=[x0;y0;z0];   % 观测站的位置
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
for t=2:N
    X(:,t)=F*X(:,t-1)+sqrtm(Q)*randn(9,1);  % 目标真实轨迹
end

for t=1:N
    % Z(t)=hfun(X(:,t),Xstation)+w(t);          % 对目标的观测
    [dd,alpha,beta]=funh(X(:,t),Xstation);
    Z(:,t) = [dd,alpha,beta]' + sqrtm(R)*randn(3,1);
end

% EKF滤波
Xekf=zeros(9,N);   % EKF滤波值
Xekf(:,1)=X(:,1);  % EKF滤波初始化
P0 =[100 0 0 0 0 0 0 0 0;             % 协方差初始化
     0 100 0 0 0 0 0 0 0;
     0 0 100 0 0 0 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 0 0 0;
     0 0 0 0 0 0 0.1 0 0;
     0 0 0 0 0 0 0 0.1 0
     0 0 0 0 0 0 0 0 0.1];
 
for i=2:N
    Xn=F*Xekf(:,i-1);            % 预测
    P1=F*P0*F'+ Q;               % 预测误差协方差   没有G就不写
    
    [dd,alpha,beta]=funh(Xn,Xstation);   % 观测预测
    % 求Jacobian矩阵H，H为3行9列的矩阵
    D = Dist(Xn,Xstation);
    DD = Dist3(Xn,Xstation);
    H = [(Xn(1,1)-x0)/DD,(Xn(2,1)-y0)/DD,(Xn(3,1)-z0)/DD,0,0,0,0,0,0;
          -(Xn(2,1)-y0)/D^2,(Xn(1,1)-x0)/D^2,0,0,0,0,0,0,0;
          (1/(1+((Xn(3,1)-z0)/D)^2)).*(-2*(Xn(1,1)-x0)/D^4),(1/(1+((Xn(3,1)-z0)/D)^2)).*(-2*(Xn(2,1)-y0)/D^4),(1/D)/(1/(1+((Xn(3,1)-z0)/D)^2)),0,0,0,0,0,0];
  
    K=P1*H'*inv(H*P1*H'+R);                    % kalman增益
    Xekf(:,i)=Xn+K*(Z(:,i)-[dd,alpha,beta]');  % 状态更新
    P0=(eye(9)-K*H)*P1;                        % 协方差更新
end

Err_KalmanFilter = zeros(1,N);
for i=1:N
  Err_KalmanFilter(i)= (Dist3(X(:,i),Xekf(:,i)));
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% 画图，轨迹图
figure
t=1:N;
hold on;
box on;
grid on;
plot3(X(1,t),X(2,t),X(3,t),'k-');
plot3(Z(1,t).*cos(Z(3,t)).*cos(Z(2,t)),Z(1,t).*cos(Z(3,t)).*sin(Z(2,t)),Z(1,t).*sin(Z(3,t)),'-b.');
plot3(Xekf(1,t),Xekf(2,t),Xekf(3,t),'-r.');
legend('实际值','测量值','ekf估计值');
xlabel('x方向位置/米')
ylabel('y方向位置/米')
zlabel('z方向位置/米')
view(3)


figure
hold on;
box on;
plot(Err_KalmanFilter,'-ks','MarkerFace','r');
legend('跟踪误差EKF');

%figure 
%hold on;
%box on;
%plot(Z/pi*180,'-r.','MarkerFace','r');
%plot(Z/pi*180+w/pi*180,'-ko','MarkerFace','g');
%legend('真实角度','观测角度');

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function cita=hfun(X1,X0)
% 先判断飞机在观测站的哪个方向
if X1(3,1)-X0(2,1)>=0  % 上
    if X1(1,1)-X0(1,1)>0  % 右上
        cita=atan(abs( (X1(3,1)-X0(2,1))/(X1(1,1)-X0(1,1)) ));
    elseif X1(1,1)-X0(1,1)==0 % 正上
        cita=pi/2;
    else % 左上
        cita=pi/2+atan(abs( (X1(3,1)-X0(2,1))/(X1(1,1)-X0(1,1)) ));
    end
else  % 下
    if X1(1,1)-X0(1,1)>0  % 右下
        cita=3*pi/2+atan(abs( (X1(3,1)-X0(2,1))/(X1(1,1)-X0(1,1)) ));
    elseif X1(1,1)-X0(1,1)==0 % 正下
        cita=3*pi/2;
    else % 左下
        cita=pi+atan(abs( (X1(3,1)-X0(2,1))/(X1(1,1)-X0(1,1)) ));
    end
end

%%%%%%%%%%%%% 求两点部分距离（三维）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 );



%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% 观测函数
% 输入：飞机和观测站的坐标
% 输出：dd为飞机到观测站的距离
%       alpha为观测站到飞机的偏航角
%       beta为观测站到飞机的俯仰角
function [dd,alpha,beta]=funh(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);
alpha = atan((X1(2,1)-X0(2,1))/(X1(1,1)-X0(1,1)));
beta = atan((X1(3,1)-X0(3,1))/sqrt((X1(1,1)-X0(1,1))^2+(X1(2,1)-X0(2,1))^2));