无线传感器网络跟踪定位卡尔曼滤波matlab源码CA

%卡尔曼滤波自编源码实现，Kalman-CAclearallT=1%离线计算的矩阵forn=1:40Q1(:,:,n)=2.5^2*eye(2)Q2(:,:,n)=[3^2002^2]endF=[1TT^2/200001T0000010000001TT^2/200001T000001]G=[T*T^2/40T^2/20T00T*T^2/40T^2/20T]C=[100000000100]%循环10次，蒙特卡洛统计formonte=1:10%初始值x(:,:,1)=[0,25,10,0,20,10]'%第一时刻的实际位置，实际的速度值,加速度值v1(:,:,1)=[normrnd(0,2.5^2,1,1),normrnd(0,2.5^2,1,1)]'x(:,:,2)=F*x(:,:,1)+G*v1(:,:,1)%实际位置，实际的速度值,加速度值v1(:,:,2)=[normrnd(0,2.5^2,1,1),normrnd(0,2.5^2,1,1)]'x(:,:,3)=F*x(:,:,2)+G*v1(:,:,2)%实际位置，实际的速度值,加速度值z(:,:,1)=[x(1,1,1)+normrnd(0,9,1,1),x(4,1,1)+normrnd(0,4,1,1)]'%观测值，观测到的位置z(:,:,2)=[x(1,1,2)+normrnd(0,9,1,1),x(4,1,2)+normrnd(0,4,1,1)]'%观测值，观测到的位置z(:,:,3)=[x(1,1,3)+normrnd(0,9,1,1),x(4,1,3)+normrnd(0,4,1,1)]'%观测值，观测到的位置xe(:,:,2)=x(:,:,2)%假设赋给这个值xe(:,:,3)=[z(1,1,3),(z(1,1,3)-z(1,1,2))/T,((z(1,1,3)-z(1,1,2))/T-(z(1,1,2)-z(1,1,1))/T)/T,z(2,1,3),(z(2,1,3)-z(2,1,2))/T,((z(2,1,3)-z(2,1,2))/T-(z(2,1,2)-z(2,1,1))/T)/T]'xp(:,:,1)=[000000]'p(:,:,3)=[9,9/T,9/T^2,0,0,09/T,18/T^2,27/(T*T^2),0,0,09/T^2,27/(T*T^2),54/(T^2*T^2),0,0,00,0,0,4,4/T,4/T^20,0,0,4/T,8/T^2,12/(T*T^2)0,0,0,4/T^2,8/(T*T^2),24/(T^2*T^2)]%卡尔曼滤波的过程forn=4:40%还是实际值和观测值v1(:,:,n-1)=[normrnd(0,2.5^2,1,1),normrnd(0,2.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(4,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(6)-K(:,:,n)*C)*pt(:,:,n)end%为展示跟踪曲线做准备%fori=1:40%xtr(i)=x(1,1,i)%ytr(i)=x(4,1,i)%px(i)=xp(1,1,i)%py(i)=xp(4,1,i)%ex(i)=xe(1,1,i)%ey(i)=xe(4,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(4,1,k)-xp(4,1,k))^2)de(k,monte)=sqrt((x(1,1,k)-xe(1,1,k))^2+(x(4,1,k)-xe(4,1,k))^2)dl(k,monte)=sqrt((x(1,1,k)-z(1,1,k))^2+(x(4,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:40plot(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')