用雷達跟蹤目標，目標的運動可以看成是在徑向和橫向內的二維運動，其運動方程和觀測方程分別為：s1(k)、v1(k)和y1(k)分別為徑向距離、速度和觀測值，而s2(k)、v2(k)和y2(k)分別為橫向距離、速度和觀測值。u1(k)和u2(k)是狀態(tài)噪聲，是目標速度的波動；w1(k)和w2(k)是觀測噪聲；四種噪聲的均值都為0，呈高斯分布，互不相關。狀態(tài)方程和輸出方程：其中：在推導上式時，wi(k)和wi(k+1)是隨機過程中不同時刻的兩個隨機變量，我們認為這兩個隨機變量統(tǒng)計獨立，而且w(k)是平穩(wěn)隨機過程，其不同時刻的方差相同。兩個時刻的時差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=[ %狀態(tài)噪聲協(xié)方差矩陣 0,0,0,0; 0,sigma_u1,0,0; 0,0,0,0; 0,0,0,sigma_u2; ];R=[ %觀測噪聲協(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)濾波估計值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);%狀態(tài)預測方程 p1=A*p*A'+Q; %狀態(tài)預測誤差協(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('徑向距離觀測值','徑向距離估計值','Location','southeast');title('二維卡爾曼濾波器');figure(5)plot(t,y(2,:),'b',t,x_calculation(3,:),'r');xlabel('t');ylabel('s2');legend('橫向距離觀測值','橫向距離估計值','Location','southeast');
文檔鏈接：北太天元文檔使用了Gitee，加載速度一般般。目前官方文檔不太多，就找到這些了。2024年更新，Gitee寄了。換了AtomGit，已經可以正常訪問。
公眾號