擴展卡爾曼濾波matlab程序

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

2、onal terms in Taylor's series.clc; close all ; clear all ;Xint_v = 1; 0; 0; 0; 0;wk = 1 0 0 0 0;vk = 1 0 0 0 0;for ii = 1:1:length(Xint_vAp(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 = cov(Vk;C = 1 0 0 0 0;n = 100;YY XX

3、= EKLMNFTR1(Ap,Xint_v,Uk,Qu,Vk,Qv,C,n,Wk,W,V;for it = 1:1:length(XXMSE(it = YY(it - XX(it;endtt = 1:1:length(XX;figure(1; subplot(211; plot(XX; title('ORIGINAL SIGNAL' subplot(212; plot(YY; title('ESTIMATED SIGNAL'figure(2; plot(tt,XX,tt,YY; title('Combined plot'legend('o

4、riginal' , 'estimated' ;figure(3; plot(MSE.2; title('Mean 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-1Ap(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&

5、#39;for ii = 1:1:length(Xint_vXKk(ii,1 = Xint_v(ii2; % FIRST STEPendPPk = Ap*PPk*Ap' % SECOND STEPKk = PPk*C'*inv( (C*PPk*C' + (V*Qv*V' ; % THIRD STEPfor ii = 1:1:length(Xint_vXUPK(ii,1 = XKk(ii2 + UUk(ii; % UPPER EQUATIONS.Zk(ii,1 = cos(XUPK(ii + VVk(ii; % UPPER EQUATIONS.endfor ii

6、= 1:1:length(XKkXBARk(ii,1 = XKk(ii + Kk(ii*(Zk(ii - (cos(XKk(ii ; % FOURTH STEPendII = eye(5,5;Pk = ( II - Kk*C*PPk; % FIFTH STEP%-%-for ii = 1:1:nUUk = Uk(ii+1; 0; 0; 0; 0;PPk = XBARk*XBARk'VVk = Vk(ii+1; 0; 0; 0; 0;XKk = exp(-XBARk; % FIRST STEPPPkM = Ap*PPk*Ap' % SECOND STEPKk = PPkM*C'*inv( (C*PPkM*C' + (V*Qv*V' ; % THIRD STEPfor nn = 1:1:length(XBARkXUPK(nn = exp(-XKk(nn + UUk(nn;% UPPER EQUATIONS.Zk(nn = cos(XUPK(nn + VVk(nn;% UPPER EQUATIONS.endfor in = 1:1:length(XUPKXNEW(in = XBARk(in + Kk(in*(Zk(in - cos(XBARk(in; % FOURTH STEPendII = eye(5