擴(kuò)展信息濾波matlab代碼

1、精選優(yōu)質(zhì)文檔-傾情為你奉上%EIF 擴(kuò)展信息濾波function eifclearclose allclcN=100; %采樣點(diǎn)數(shù)T=0.1; %采樣時(shí)間x(:,1)=10;50;5;10;2;4; xg(:,1)=x(:,1); %狀態(tài)賦初值P=diag(1,1,1,1,1,1); %協(xié)方差矩陣 賦初值Y=inv(P);yg(:,1)=Y*xg(:,1);Q=diag(0.02,0.02,0.012,0.012,0.052,0.052); %過程噪聲協(xié)方差矩陣R=diag(0.4,0.01); %觀測(cè)噪聲協(xié)方差矩陣v=sqrt(R)*randn(2,N,1); %觀測(cè)噪聲序列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; %狀態(tài)矩陣%模擬觀測(cè)值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%計(jì)算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); %求雅可比矩陣 計(jì)算輸出矩陣 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)' %狀態(tài)誤差均方值end k=1:N; %繪制噪聲曲線 figure plot(k,w) title('過程噪聲強(qiáng)度曲線') figure plot(k,v) title('觀測(cè)噪聲強(qiáng)度曲線') %繪制狀態(tài)誤差曲線 figure subplot(2,1

5、,1) plot(k,err(1,:) title('橫坐標(biāo)誤差曲線') subplot(2,1,2) plot(k,err(2,:) title('縱坐標(biāo)誤差曲線') 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('縱向加速度誤差曲線') %繪制狀態(tài)誤差均方值曲線 figure subplot(3,2,1) plot(k,Ex(1,:) title('橫坐標(biāo)誤差均方值曲線') subplot(3,2,2) plot(k,Ex(2,:) title('縱坐標(biāo)誤差均方值曲線') 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 %繪制真實(shí)軌跡hold onfor i=1:N plot(xg(1,i),xg(2,i),'-ks') hold onend %繪制ekf最優(yōu)估計(jì) 軌跡endfunction A = my_jacobian( x1,x2