扩展信息滤波matlab代码

1、精选优质文档-倾情为你奉上%EIF 扩展信息滤波function eifclearclose allclcN=100; %采样点数T=0.1; %采样时间x(:,1)=10;50;5;10;2;4; xg(:,1)=x(:,1); %状态赋初值P=diag(1,1,1,1,1,1); %协方差矩阵 赋初值Y=inv(P);yg(:,1)=Y*xg(:,1);Q=diag(0.02,0.02,0.012,0.012,0.052,0.052); %过程噪声协方差矩阵R=diag(0.4,0.01); %观测噪声协方差矩阵v=sqrt(R)*randn(2,N,1); %观测噪声序列w=sqrt(Q

2、)*randn(6,N,1); %过程噪声序列I=eye(6); %6*6单位矩阵阵F=1 0 T 0 T2/2 0; 0 1 0 T 0 T2/2; 0 0 1 0 T 0; 0 0 0 1 0 T; 0 0 0 0 1 0; 0 0 0 0 0 1; %状态矩阵%模拟观测值for k=2:N x(:,k)=F*x(:,k-1)+w(:,k); z(:,k)=sqrt(x(1,k)2+x(2,k)2) atan(x(2,k)/x(1,k)'+v(:,k);end%计算for k=2:N xk(:,k)=F*xg(:,k-1); Y=inv(F*inv(Y)*F'+Q); yk

3、(:,k)=Y*xk(:,k); %KF 方程1 % C=xk(1,k)/sqrt(xk(1,k)2+xk(2,k)2) xk(2,k)/sqrt(xk(1,k)2+xk(2,k)2) 0 0 0 0; % -xk(2,k)/(xk(1,k)2+xk(2,k)2) xk(1,k)/(x(1,k)2+xk(2,k)2) 0 0 0 0; C=my_jacobian(xk(1,k),xk(2,k); %求雅可比矩阵 计算输出矩阵 Fai=C'/R*C; ik(:,k)=C'/R*(z(:,k)-sqrt(xk(1,k)2+xk(2,k)2);atan(xk(2,k)/xk(1,k)

4、+C*xk(:,k); Y=Y+Fai; yg(:,k)=yk(:,k)+ik(:,k); P=inv(Y); xg(:,k)=P*yg(:,k); err(:,k)=x(:,k)-xg(:,k); %误差 Ex(:,k)=P(1,1) P(2,2),P(3,3),P(4,4),P(5,5),P(6,6)' %状态误差均方值end k=1:N; %绘制噪声曲线 figure plot(k,w) title('过程噪声强度曲线') figure plot(k,v) title('观测噪声强度曲线') %绘制状态误差曲线 figure subplot(2,1

5、,1) plot(k,err(1,:) title('横坐标误差曲线') subplot(2,1,2) plot(k,err(2,:) title('纵坐标误差曲线') figure subplot(2,1,1) plot(k,err(3,:) title('横向速度误差曲线') subplot(2,1,2) plot(k,err(4,:) title('纵向速度误差曲线') figure subplot(2,1,1) plot(k,err(5,:) title('横向加速度误差曲线') subplot(2,1,2

6、) plot(k,err(6,:) title('纵向加速度误差曲线') %绘制状态误差均方值曲线 figure subplot(3,2,1) plot(k,Ex(1,:) title('横坐标误差均方值曲线') subplot(3,2,2) plot(k,Ex(2,:) title('纵坐标误差均方值曲线') subplot(3,2,3) plot(k,Ex(3,:) title('横向速度误差均方值曲线') subplot(3,2,4) plot(k,Ex(4,:) title('纵向速度误差均方值曲线') subplot(3,2,5) plot(k,Ex(5,:) title('横向加速度误差均方值曲线') subplot(3,2,6) plot(k,Ex(6,:) title('纵向加速度误差均方值曲线') figurefor i=1:N plot(x(1,i),x(2,i),'*') hold onend %绘制真实轨迹hold onfor i=1:N plot(xg(1,i),xg(2,i),'-ks') hold onend %绘制ekf最优估计 轨迹endfunction A = my_jacobian( x1,x2