擴(kuò)展卡爾曼濾波matlab程序

1、文件一：% THIS PROGRAM IS FOR IMPLEMENTATION OF DISCRETE TIME PROCESS EXTENDED KALMAN FILTER% FOR GAUSSIAN AND LINEAR STOCHASTIC DIFFERENCE EQUATION.% By (R.C.R.C.R),SPLABS,MPL.% (17 JULY 2005).% Help by Aarthi Nadarajan is acknowledged.% (drawback of EKF is when nonlinearity is high, we can extend the

2、% approximation taking additional terms in Taylors series).clc; close all; clear all;Xint_v = 1; 0; 0; 0; 0;wk =F 0 0 0 0;vk = 1 0 0 0 0;for ii = 1:1:length(Xint_v)Ap(ii) = Xint_v(ii)*2;W(ii) = 0;H(ii) = -sin(Xint_v(ii);V(ii) = 0;Wk(ii) = 0;endUk = randn(1,200);Qu = cov(Uk);Vk = randn(1,200);Qv = co

3、v(Vk);C = 1 0 0 0 0;n = 100;YY XX = EKLMNFTR1(Ap,Xint_v,Uk,Qu,Vk,Qv,C,n,Wk,W,V);for it = 1:1:length(XX)MSE(it) = YY(it) - XX(it);end tt = 1:1:length(XX);figure(1); subplot(211); plot(XX); titleORIGINAL SIGNAL);subplot(212); plot(YY); titleESTIMATED SIGNAL);figure(2); plot(tt,XX,tt,YY); title(Combine

4、d plot); legend(original,estimated);figure(3); plot(MSE.八2); titleMean square error);子文件:：function YY,XX = EKLMNFTR1(Ap,Xint_v,Uk,Qu,Vk,Qv,C,n,Wk,W,V);Ap(2,:) = 0;for ii = 1:1:length(Ap)-1 Ap(ii+1,ii) = 1;endinx = 1;UUk = Uk(inx); 0; 0; 0; 0;PPk = (Xint_v*Xint_v);VVk = Vk(inx); 0; 0; 0; 0;Qv = V*V;f

5、or ii = 1:1:length(Xint_v)% FIRST% SECOND STEP% THIRD STEP% UPPER% UPPERXKk(ii,1) = Xint_v(ii)八2;STEP endPPk = Ap*PPk*Ap;Kk = PPk*C*inv( (C*PPk*C) + (V*Qv*V);for ii = 1:1:length(Xint_v)XUPK(ii,1) = XKk(ii)八2 + UUk(ii); EQUATIONS.Zk(ii,1) = cos(XUPK(ii) + VVk(ii); EQUATIONS.end for ii = 1:1:length(XK

6、k)XBARk(ii,1) = XKk(ii) + Kk(ii)*(Zk(ii) - (cos(XKk(ii) ;% FOURTHSTEP endII = eye(5,5);Pk = ( II - Kk*C)*PPk;% FIFTH STEP%for ii1:1:nUUk = Uk(ii+1); 0; 0; 0; 0;PPk = XBARk*XBARk;VVk = Vk(ii+1); 0; 0; 0; 0;% FIRST STEP% SECOND STEP% THIRD STEP% UPPER% UPPERXKk = exp(-XBARk);PPkM = Ap*PPk*Ap;Kk = PPkM*C*inv( (C*PPkM*C) + (V*Qv*V);for nn = 1:1:length(XBARk)XUPK(nn) = exp(-XKk(nn) + UUk(nn); EQUATIONS.Zk(nn) = cos(XUPK(nn) + VVk(nn); EQUATIONS.endfor in = 1:1:length(XUPK)% FOURTHXNEW(in) = XBARk(in) + Kk(in)*(Zk(in) - cos(XBARk(in); STEP endII = eye(5,5);% F