二维卡尔曼滤波器仿真

用雷达跟踪目标，目标的运动可以看成是在径向和横向内的二维运动，其运动方程和观测方程分别为：

s1(k)、v1(k)和y1(k)分别为径向距离、速度和观测值，而s2(k)、v2(k)和y2(k)分别为横向距离、速度和观测值。u1(k)和u2(k)是状态噪声，是目标速度的波动；w1(k)和w2(k)是观测噪声；四种噪声的均值都为0，呈高斯分布，互不相关。

状态方程和输出方程：

其中：

在推导上式时，wi(k)和wi(k+1)是随机过程中不同时刻的两个随机变量，我们认为这两个随机变量统计独立，而且w(k)是平稳随机过程，其不同时刻的方差相同。两个时刻的时差T为雷达扫描一圈的时间。

仿真结果如下：

clear allclose allclcT=1;sigma_u1=300;sigma_u2=0.12;sigma_w1=900000;sigma_w2=900000;N=500;u1=sqrt(sigma_u1).*randn(1,N);u2=sqrt(sigma_u2).*randn(1,N);w1=sqrt(sigma_w1).*randn(1,N);w2=sqrt(sigma_w2).*randn(1,N);A=[     1,T,0,0;    0,1,0,0;    0,0,1,T;    0,0,0,1;    ];C=[     1,0,0,0;    0,0,1,0;    ];Q=[                       %状态噪声协方差矩阵    0,0,0,0;    0,sigma_u1,0,0;    0,0,0,0;    0,0,0,sigma_u2;    ];R=[                         %观测噪声协方差矩阵    sigma_w1,0;    0,sigma_w2;    ];I=[     1,0,0,0;    0,1,0,0;    0,0,1,0;    0,0,0,1;    ];x0=[10000;300;900;256;];%初始值% y0=[10000;0;];x=zeros(4,N);y=zeros(2,N);x(:,1)=x0;% y(:,1)=y0;x_calculation=zeros(4,N);  %状态滤波估计值u=[zeros(1,N);u1;zeros(1,N);u2];w=[w1;w2];y(:,1)=C*x(:,1)+w(:,1);P2=[    sigma_w1,  sigma_w1/T,               0,         0;    sigma_w1/T,sigma_u1+2*sigma_w1/(T^2),0,         0;    0,         0,                        sigma_w2,  sigma_w2/T;    0,         0,                        sigma_w2/T,sigma_u2+2*sigma_w2/(T^2);    ];p=P2;for i=1:N-1      x(:,i+1)=A*x(:,i)+u(:,i);%真实值    y(:,i+1)=C*x(:,i+1)+w(:,i+1);                 endx_calculation(:,2)=[y(1,1) , (y(1,2)-y(1,1))/T ,y(2,2), (y(2,2)-y(2,1))/T]';for i=3:N    x1=A*x_calculation(:,i-1);%状态预测方程    p1=A*p*A'+Q;  %状态预测误差协方差矩阵    K=p1*C'*inv(C*p1*C'+R);   %最佳增益方程    x_calculation(:,i)=x1+K*(y(:,i)-C*x1);  %滤波估值方程    p=(I-K*C)*p1; %滤波估值误差 协方差方程end t=1:N; figure(4)plot(t,y(1,:),'b',t,x_calculation(1,:),'r');xlabel('t');ylabel('s1');legend('径向距离观测值','径向距离估计值','Location','southeast');title('二维卡尔曼滤波器');figure(5)plot(t,y(2,:),'b',t,x_calculation(3,:),'r');xlabel('t');ylabel('s2');legend('横向距离观测值','横向距离估计值','Location','southeast');