?? 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
?? 快捷鍵說明
復制代碼
Ctrl + C
搜索代碼
Ctrl + F
全屏模式
F11
切換主題
Ctrl + Shift + D
顯示快捷鍵
?
增大字號
Ctrl + =
減小字號
Ctrl + -