擴展卡爾曼濾波（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));