kalman_fliter_nine.m

九维的卡尔曼滤波跟踪算法

function [Z,X_yuce]=kalman_fliter_nine(A1,A2,A3,N,T,ave,var)
%%%A1,A2及A3为极坐标下测量的前三组数据，都为1*3的矩阵，为确定卡尔曼滤波的初始值和初始协方差,ave和var分别为采样间隔T1的均值和方差
% clear all;
% clc;

%%%初始条件给定
% T=1;
% N=100;
Nvar1=50;
Nvar2=60;
Nvar3=20;
Wvar1=100;
Wvar2=80;
Wvar3=50;
V1=sqrt(Nvar1)*randn(1,N);              %%%产生方差为Nvar1的正态分布的过程噪声数据
V2=sqrt(Nvar2)*randn(1,N);              %%%产生方差为Nvar2的正态分布的过程噪声数据
V3=sqrt(Nvar3)*randn(1,N);              %%%产生方差为Nvar3的正态分布的过程噪声数据
W1=sqrt(Wvar1)*randn(1,N);              %%%产生方差为Wvar1的正态分布的量测噪声数据
W2=sqrt(Wvar2)*randn(1,N);              %%%产生方差为Wvar2的正态分布的量测噪声数据
W3=sqrt(Wvar3)*randn(1,N);              %%%产生方差为Wvar3的正态分布的量测噪声数据
I=eye(9);


%%%初始状态给定
F=[1 T T^2/2 0 0 0 0 0 0
   0 1 T 0 0 0 0 0 0
   0 0 1 0 0 0 0 0 0
   0 0 0 1 T T^2/2 0 0 0
   0 0 0 0 1 T 0 0 0
   0 0 0 0 0 1 0 0 0
   0 0 0 0 0 0 1 T T^2/2
   0 0 0 0 0 0 0 1 T
   0 0 0 0 0 0 0 0 1];                %%%系统方程的一步转移矩阵
H=[T^2/2 0 0 
   T 0 0
   1 0 0
   0 T^2/2 0
   0 T 0
   0 1 0
   0 0 T^2/2
   0 0 T
   0 0 1];                            %%%系统方程的噪声输入矩阵
C=[1 0 0 0 0 0 0 0 0
   0 0 0 1 0 0 0 0 0
   0 0 0 0 0 0 1 0 0];                %%%量测方程的量测矩阵
%Q=7.5^2*eye(3,3);                    %%%过程噪声的协方差矩阵，为方阵，可设为对角阵
Q=[50 0 0
   0 60 0
   0 0 20];
R=[90 0 0
   0 40 0
   0 0 20];                          %%%量测噪声的协方差矩阵，为方阵，不一定为对角阵


% d=100;                            %%%d为径向距离测量误差的协方差
% e=0.001;                          %%%e为方位角测量误差的协方差
% f=0.001;                          %%%f为俯仰角测量误差的协方差

%%%%%此处调用轨迹函数的前三组数值
a1=A1(1);
a2=A1(2);
a3=A1(3);                         %%%a代表方位角φ
b1=A2(1);
b2=A2(2);
b3=A2(3);                         %%%b代表俯仰角θ
c1=A3(1);
c2=A3(2);
c3=A3(3);                         %%%c代表径向距离r
% a1=1.5703;
% a2=1.5698;
% a3=1.5693;
% b1=0;
% b2=0;
% b3=0;
% c1=sqrt(2000^2+1);
% c2=sqrt(2000^2+4);
% c3=sqrt(2000^2+9);
%%%以上数值假设状态向量[x x' x'' y y' y'' z z' z'']=[10 300 10 10 200 20 10 100 30]
%%%依据其前三组数值[10 10 10],[315 220 125],[630 450 270]通过坐标转换而来

%%%系统的初始状态
Z11=c1*cos(a1)*cos(b1);
Z21=c1*sin(a1)*cos(b1);
Z31=c1*sin(b1);

Z12=c2*cos(a2)*cos(b2);
Z22=c2*sin(a2)*cos(b2);
Z32=c2*sin(b2);

Z13=c3*cos(a3)*cos(b3);
Z23=c3*sin(a3)*cos(b3);
Z33=c3*sin(b3);

X0=[Z13
    (Z13-Z12)/T
    ((Z13-Z12)/T-(Z12-Z11)/T)/T
    Z23
    (Z23-Z22)/T
    ((Z23-Z22)/T-(Z22-Z21)/T)/T
    Z33
    (Z33-Z32)/T
    ((Z33-Z32)/T-(Z32-Z31)/T)/T];

% %%%初始协方差矩阵
% D=[d 0 0
%    0 e 0
%    0 0 f];
% 
% B1=[cos(a1)*cos(b1) -c1*sin(a1)*cos(b1) -c1*cos(a1)*sin(b1)
%    sin(a1)*cos(b1) c1*cos(a1)*cos(b1) -c1*sin(a1)*sin(b1)
%    sin(b1) 0 c1*cos(b1)];
% E1=B1*D*B1';
% 
% 
% B2=[cos(a2)*cos(b2) -c2*sin(a2)*cos(b2) -c2*cos(a2)*sin(b2)
%    sin(a2)*cos(b2) c2*cos(a2)*cos(b2) -c2*sin(a2)*sin(b2)
%    sin(b2) 0 c2*cos(b2)];
% E2=B2*D*B2';
% 
% 
% B3=[cos(a3)*cos(b3) -c3*sin(a3)*cos(b3) -c3*cos(a3)*sin(b3)
%    sin(a3)*cos(b3) c3*cos(a3)*cos(b3) -c3*sin(a3)*sin(b3)
%    sin(b3) 0 c3*cos(b3)];
% E3=B3*D*B3';
E1=R;
E2=R;
E3=R;


for i=1:3
    for j=1:3
        P_yuce(3*i-2:3*i,3*j-2:3*j)=[E3(i,j) E3(i,j)/T E3(i,j)/T^2
                                E3(i,j)/T (E3(i,j)+E2(i,j))/T^2 (E3(i,j)+2*E2(i,j))/T^3
                                E3(i,j)/T^2 (E3(i,j)+2*E2(i,j))/T^3 (E3(i,j)+4*E2(i,j)+E1(i,j))/T^4];
    end
end


%%%卡尔曼滤波算法%%%
X_chushi=X0;
X_yuce(:,1)=X0;

T1=ave+sqrt(var)*randn(1,N);
%%%产生均值为ave，方差为var的正态分布随机矩阵，作为不断变化的采样间隔
t=0;

for k=1:N
%     X(:,k)=F*X_chushi+H*[V1(k),V2(k),V3(k)]';
%     Z(:,k)=C*X(:,k)+[W1(k),W2(k),W3(k)]';
    t=t+T1(1,k);                                               %%%每次循环调用一组数值，而不是全部的数值
    [x y z]=fuyang(0,2000,0,2400,15,100,1,3,30,0,t);           %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%此处调用函数轨迹
    Z(:,k)=[x y z]'+[W1(k),W2(k),W3(k)]';
    X_guji(:,k)=F*X_yuce(:,k);
    P_guji=F*P_yuce*F'+H*Q*H';
    a=Z(:,k)-C*X_guji(:,k);
    A=C*P_guji*C'+R;
    K=P_guji*C'*inv(A);
    X_yuce(:,k+1)=X_guji(:,k)+K*a;
    P_yuce=[I-K*C]*P_guji;
%     X_chushi=X(:,k);
end


%%%绘图
% plot3(X(1,:),X(4,:),X(7,:))
% hold on;
% plot3(X_yuce(1,:),X_yuce(4,:),X_yuce(7,:),'r')
% subplot(3,1,1);
% plot(X_yuce(1,2:101)-X(1,:),'r')
% hold on;
% plot(X_yuce(4,2:101)-X(4,:),'-.')
% hold on;
% plot(X_yuce(7,2:101)-X(7,:),'g')
% hold on;
% subplot(3,1,2);
% plot(X_yuce(2,2:101)-X(2,:),'r')
% hold on;
% plot(X_yuce(5,2:101)-X(5,:),'-.')
% hold on;
% plot(X_yuce(8,2:101)-X(8,:),'g')
% subplot(3,1,3)
% plot(X_yuce(3,2:101)-X(3,:),'r')
% hold on;
% plot(X_yuce(6,2:101)-X(6,:),'-.')
% hold on;
% plot(X_yuce(9,2:101)-X(9,:),'g')