扩展卡尔曼滤波matlab程序.doc

文件一：： % 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 % approximation taking additional terms in Taylors 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_v) Ap(ii) = Xint_v(ii)*2; W(ii) = 0; H(ii) = -sin(Xint_v(ii)); V(ii) = 0; Wk(ii) = 0; end Uk = randn(1,200); Qu = cov(Uk); Vk = randn(1,200); Qv = cov(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); title(ORIGINAL SIGNAL); subplot(212); plot(YY); title(ESTIMATED SIGNAL); figure(2); plot(tt,XX,tt,YY); title(Combined plot); legend(original,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)-1 Ap(ii+1,ii) = 1; end inx = 1; UUk = [Uk(inx); 0; 0; 0; 0]; PPk = (Xint_v*Xint_v); VVk = [Vk(inx); 0; 0; 0; 0]; Qv = V*V; for ii = 1:1:length(Xint_v) XKk(ii,1) = Xint_v(ii)^2; % FIRST STEP end PPk = Ap*PPk*Ap; % SECOND STEP Kk = PPk*C*inv( (C*PPk*C) + (V*Qv*V) ); % THIRD STEP for ii = 1:1:length(Xint_v) XUPK(ii,1) = XKk(ii)^2 + UUk(ii); % UPPER EQUATIONS. Zk(ii,1) = cos(XUPK(ii)) + VVk(ii); % UPPER EQUATIONS. end for ii = 1:1:length(XKk) XBARk(ii,1) = XKk(ii) + Kk(ii)*(Zk(ii) - (cos(XKk(ii)))) ; % FOURTH STEP end II = eye(5,5); Pk = ( II - Kk