用雷達(dá)跟蹤目標(biāo)，目標(biāo)的運(yùn)動(dòng)可以看成是在徑向和橫向內(nèi)的二維運(yùn)動(dòng)，其運(yùn)動(dòng)方程和觀測(cè)方程分別為：

s1(k)、v1(k)和y1(k)分別為徑向距離、速度和觀測(cè)值，而s2(k)、v2(k)和y2(k)分別為橫向距離、速度和觀測(cè)值。u1(k)和u2(k)是狀態(tài)噪聲，是目標(biāo)速度的波動(dòng)；w1(k)和w2(k)是觀測(cè)噪聲；四種噪聲的均值都為0，呈高斯分布，互不相關(guān)。

狀態(tài)方程和輸出方程：

其中：

在推導(dǎo)上式時(shí)，wi(k)和wi(k+1)是隨機(jī)過(guò)程中不同時(shí)刻的兩個(gè)隨機(jī)變量，我們認(rèn)為這兩個(gè)隨機(jī)變量統(tǒng)計(jì)獨(dú)立，而且w(k)是平穩(wěn)隨機(jī)過(guò)程，其不同時(shí)刻的方差相同。兩個(gè)時(shí)刻的時(shí)差T為雷達(dá)掃描一圈的時(shí)間。

仿真結(jié)果如下：

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=[                       %狀態(tài)噪聲協(xié)方差矩陣    0,0,0,0;    0,sigma_u1,0,0;    0,0,0,0;    0,0,0,sigma_u2;    ];R=[                         %觀測(cè)噪聲協(xié)方差矩陣    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);  %狀態(tài)濾波估計(jì)值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);%真實(shí)值    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);%狀態(tài)預(yù)測(cè)方程    p1=A*p*A'+Q;  %狀態(tài)預(yù)測(cè)誤差協(xié)方差矩陣    K=p1*C'*inv(C*p1*C'+R);   %最佳增益方程    x_calculation(:,i)=x1+K*(y(:,i)-C*x1);  %濾波估值方程    p=(I-K*C)*p1; %濾波估值誤差 協(xié)方差方程end t=1:N; figure(4)plot(t,y(1,:),'b',t,x_calculation(1,:),'r');xlabel('t');ylabel('s1');legend('徑向距離觀測(cè)值','徑向距離估計(jì)值','Location','southeast');title('二維卡爾曼濾波器');figure(5)plot(t,y(2,:),'b',t,x_calculation(3,:),'r');xlabel('t');ylabel('s2');legend('橫向距離觀測(cè)值','橫向距離估計(jì)值','Location','southeast');