ekf.m

高速车辆组合导航研究

function [plotP,truer,truev,r,v,T,tf] = EKF()
%  Extended Kalman Filter ,descrete and non-linear（the programme is for 
%  the example of P192 of the book《天文导航原理及应用》 房建成 宁晓林 编著）
%  Nov-29-2007
% Input:
%      X0: the initial state vector
%      P0: the initial error covariance

% Output: 
%      plorP: keep the covariance matirx for plotting
%      truer: keep the true position(diameter of the satellite) for
%             plotting
%      T:     the time interval
%      tf:    the simulation time 
% Reference:
%          最佳估计理论 陈新海 编
%          第六章 非线性滤波 6.3推广的kalman滤波
clc
clear
T = 3;% the interval time
tf = 45000;%the whole simulation time
X0 = [4.590e6,4.388e6,3.228e6,-4.612e3,5.014e2,5.876e3]';% initial true state;
N = length(X0);%get the length of A
P0 = diag([400^2,400^2,400^2,0.8^2,0.8^2,0.8^2]);
Q=diag([(1e-3)^2,(2e-3)^2,(1e-3)^2,(1e-3)^2,(2e-3)^2,(1e-3)^2]);%process noise covriance
Rnoise=80^2;%measurement noise covariance

%simulation the system
P = P0;
X = X0;
Z = [];
A = [];
[Z,truer,truev] = GetZ(X0,T,tf,Q);                                                                                                                                                                                   
i = 1;

for t = 0:T:tf 
%save X,P for drawing picture    
plotx(:,i) = X;
plotP(:,i) = diag(P);

r(i) = sqrt(X(1)^2+X(2)^2+X(3)^2);%radius of the satellite orbit (positon)
v(i) = sqrt(X(4)^2+X(5)^2+X(6)^2);%velocity of the satellite

%ekf simulation 
A = GetA(X);
H = GetH(X);
Fi = eye(N) + A*T;
f = Getf(X);

Pmin = Fi*P*Fi'+Q;
K = Pmin*H'*inv(H*Pmin*H'+Rnoise);
Xmin = X+f*T+A*f*T^2/2;
h = Getlittleh(Xmin);
X = Xmin + K*(Z(i)-h);
P = (eye(N)-K*H)*Pmin;


i = i+1;
end