扩展卡尔曼滤波数据融合应用

N=200; %计算连续N个时刻 dt=0.1; %微分时间 n=2; %状态维度 q=0.3; %过程标准差（待定） r=0.05; %测量标准差（待定） Q=q^2*eye(n); %过程方差 R=r^2; %测量值的方差 ax=load('C:\Users\Brilliance\Desktop\ax.txt'); %9250xy的加速度值 ay=load('C:\Users\Brilliance\Desktop\ay.txt'); uwb_place_x=load('C:\Users\Brilliance\Desktop\place_x.txt'); uwb_place_y=load('C:\Users\Brilliance\Desktop\place_y.txt'); %测量值（位移值） last_x_speed=0; last_y_speed=0; speed_x_now=0; speed_y_now=0; Ax=0; Ay=0; f=@(x)[x(1)+last_x_speed*dt+Ax*dt^2/2;x(2)+last_y_speed*dt+Ay*dt^2/2]; %状态方程 h=@(x)[x(1);x(2)]; %测量方程 s=[0,0]; %初始状态 %初始化状态 x=s+q*randn(1,2); P = eye(n); xV = zeros(n,N); sV = zeros(n,N); zV = zeros(n,N); for k=1:N Ax = ax(k); Ay = ay(k); s(1)=uwb_place_x(k); s(2)=uwb_place_y(k); z = h(s) + r*randn; sV(:,k)= s; %实际状态 zV(:,k)= z; %状态测量值 [x1,A]=jaccsd(f,x); %计算f的雅可比矩阵，其中x1对应黄金公式line2 P=A*P*A'+Q; %过程方差预测，对应line3 [z1,H]=jaccsd(h,x1); %计算h的雅可比矩阵 K=P*H'*inv(H*P*H'+R); %卡尔曼增益，对应line4 x=x1+K*(z-z1); %状态EKF估计值，对应line5 P=P-K*H*P; %EKF方差，对应line6 xV(:,k) = x; %save s = f(s) + q*randn(2,1); %update process speed_x_now = last_x_speed + Ax*dt; speed_y_now = last_y_speed + Ay*dt; last_x_speed = speed_x_now; last_y_speed = speed_y_now; end for k=1:2 FontSize=14; LineWidth=1; figure(); plot(sV(k,:),'g-'); %画出真实值 hold on; plot(xV(k,:),'b-','LineWidth',LineWidth) %画出最优估计值 hold on; plot(zV(k,:),'k+'); %画出状态测量值 hold on; legend('UWB值', 'EKF最优估计估计值','状态测量值'); xl=xlabel('时间(秒)'); t=['状态 ',num2str(k)] ; yl=ylabel(t); set(xl,'fontsize',FontSize); set(yl,'fontsize',FontSize); hold off; set(gca,'FontSize',FontSize); end