扩展信息滤波matlab代码.docVIP

%EIF 扩展信息滤波 function eif clear close all clc N=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.01^2,0.01^2,0.05^2,0.05^2]); %过程噪声协方差矩阵 R=diag([0.4,0.01]); %观测噪声协方差矩阵 v=sqrt(R)*randn(2,N,1); %观测噪声序列 w=sqrt(Q)*randn(6,N,1); %过程噪声序列 I=eye(6); %6*6单位矩阵阵 F=[1 0 T 0 T^2/2 0; 0 1 0 T 0 T^2/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];