搜索
%卡尔曼滤波自编源码实现，Kalman-CVclearall;T=1;%离线计算的矩阵forn=1:40Q1(:,:,n)=2.5^2*eye(2);Q2(:,:,n)=[3^2002^2];endF=[1T000100001T0001];G=[T^2/20T00T^2/20T];C=[10000010];%循环100次，蒙特卡洛统计formonte=1:10%初始值x(:,:,1)=[0,5,0,3]';v1(:,:,1)=[normrnd(0,1.5^2,1,1),normrnd(0,1.5^2,1,1)]';x(:,:,2)=F*x(:,:,1)+G*v1(:,:,1);%实际位置，实际的速度值z(:,:,1)=[x(1,1,1)+normrnd(0,9,1,1),x(3,1,1)+normrnd(0,4,1,1)]';%观测值，观测到的位置z(:,:,2)=[x(1,1,2)+normrnd(0,9,1,1),x(3,1,2)+normrnd(0,4,1,1)]';%观测值，观测到的位置xe(:,:,2)=[z(1,1,2),(z(1,1,2)-z(1,1,1))/T,z(2,1,2),(z(2,1,2)-z(2,1,1))/T]';xp(:,:,1)=[0000]';xp(:,:,2)=[0000]';p(:,:,2)=[Q2(1,1,2),Q2(1,1,2)/T,0,0Q2(1,1,2)/T,2*Q2(1,1,2)/T^2,0,00,0,Q2(2,2,2),Q2(2,2,2)/T0,0,Q2(2,2,2)/T,2*Q2(2,2,2)/T^2];%卡尔曼滤波的过程forn=3:40%还是实际值和观测值v1(:,:,n-1)=[normrnd(0,1.5^2,1,1),normrnd(0,1.5^2,1,1)]';x(:,:,n)=F*x(:,:,n-1)+G*v1(:,:,n-1);%实际位置，实际的速度值z(:,:,n)=[x(1,1,n)+normrnd(0,9,1,1),x(3,1,n)+normrnd(0,4,1,1)]';%观测值，观测到的位置%开始滤波xp(:,:,n)=F*xe(:,:,n-1);a(:,:,n)=z(:,:,n)-C*xp(:,:,n);pt(:,:,n)=F*p(:,:,n-1)*F'+G*Q1(:,:,n-1)*G';A(:,:,n)=C*pt(:,:,n)*C'+Q2(:,:,n);K(:,:,n)=pt(:,:,n)*C'*inv(A(:,:,n));xe(:,:,n)=xp(:,:,n)+K(:,:,n)*a(:,:,n);p(:,:,n)=(eye(4)-K(:,:,n)*C)*pt(:,:,n);end%为展示跟踪曲线做准备fori=1:30xtr(i)=x(1,1,i);ytr(i)=x(3,1,i);px(i)=xp(1,1,i);py(i)=xp(3,1,i);ex(i)=xe(1,1,i);ey(i)=xe(3,1,i);zx(i)=z(1,1,i);zy(i)=z(2,1,i);end%计算每一次循环的滤波后的每一采样时刻的预测误差与估计误差，并为下面求所有这些次仿真的各采样时刻的误差平均值做准备fork=1:40dp(k,monte)=sqrt((x(1,1,k)-xp(1,1,k))^2+(x(3,1,k)-xp(3,1,k))^2);de(k,monte)=sqrt((x(1,1,k)-xe(1,1,k))^2+(x(3,1,k)-xe(3,1,k))^2);dl(k,monte)=sqrt((x(1,1,k)-z(1,1,k))^2+(x(3,1,k)-z(2,1,k))^2);endend%求所有这些次仿真的各采样时刻的误差平均值,并展示误差曲线forj=1:40ave_dp(j)=mean(dp(j,:));ave_de(j)=mean(de(j,:));ave_dl(j)=mean(dl(j,:));endave_total_dp=mean(ave_dp);ave_total_de=mean(ave_de);ave_total_dl=mean(ave_dl);step=1:40;plot(step,ave_dp,'r-*',step,ave_de,'b-o',step,ave_dl,'g-d');%查看跟踪轨迹px,py,'k-+',%figure(1)%plot(xtr,ytr,'r-*',zx,zy,'g-d',ex,ey,'b-o')