扩展卡尔曼滤波matlab程序

文件一

% 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 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_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*C)*PPk; % FIFTH STEP

%‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐

%‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐

for ii = 1:1:n

UUk = [Uk(ii+1); 0; 0; 0; 0];

PPk = XBARk*XBARk';

VVk = [Vk(ii+1); 0; 0; 0; 0];

XKk = exp(‐XBARk); % FIRST STEP

PPkM = Ap*PPk*Ap'; % SECOND STEP

Kk = PPkM*C'*inv( (C*PPkM*C') + (V*Qv*V') ); % THIRD STEP

for nn = 1:1:length(XBARk)

XUPK(nn) = exp(‐XKk(nn)) + UUk(nn); % UPPER EQUATIONS.