扩展卡尔曼滤波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.
Zk(nn) = cos(XUPK(nn)) + VVk(nn); % UPPER EQUATIONS.
end
for in = 1:1:length(XUPK)
XNEW(in) = XBARk(in) + Kk(in)*(Zk(in) ‐ cos(XBARk(in))); % FOURTH STEP
end
II = eye(5,5);
Pk = (II ‐ Kk*C)*PPkM; % FIFTH STEP
XBARk = XNEW;
OUTX(ii) = XBARk(1,1);
OUTY(ii) = Zk(1,1);
end
YY = OUTY;
XX = OUTX;。