ekfyognyudaohang.m

实现EKF滤波在导航重点的应用

% 扩展卡尔曼滤波算法程序
% ym和Pm分别为函数EKF求得的最优滤波值和滤波误差方差阵
function [ym, Pm] = EKF(P, y, Rm, Rs, Z, yubao_y, deta)

% Qk: 系统过程噪声方差阵
% 因为Qk的特殊性，在卡尔曼滤波的时候得适当加大
% J2和全模型加速度相差1e-6km/s^2，即系统误差为1e-6km/s^2
% Qk = diag([1e-0 1e-0 1e-0 1e-4 1e-4 1e-4]);
Qk = diag([1e-6 1e-6 1e-6 1e-10 1e-10 1e-10]);

Re = 6378.14;     % Re: 地球的平均赤道半径 && 单位: km
mu = 398600.44;	  % mu: 地心引力常数 && 单位: km^3/s^2
J2 = 1.08263e-3;  % J2: 带谐项系数
kk = pi/180;

% ======================== 求解状态转移矩阵 ========================
x1 = y(1);
x2 = y(2);
x3 = y(3);
x4 = y(4);
x5 = y(5);
x6 = y(6);
r = norm(y(1:3));  % r: 卫星的地心距 && y: ????
temp = J2*Re^2;
f41 = -mu*(r^2 - 3*x1^2 + 1.5*temp*(r^2-5*x1^2)/r^2 - 7.5*temp*x3^2*(r^2-7*x1^2)/r^4)/r^5;
f42 = mu*x1*x2*(3 + 7.5*temp*(r^2 - 7*x3^2)/r^4)/r^5;
f43 = mu*x1*x3*(3 + 7.5*temp*(3*r^2 - 7*x3^2)/r^4)/r^5;
f51 = f42;
f52 = -mu*(r^2 - 3*x2^2 + 1.5*temp - 7.5*temp*((x2^2 + x3^2)*r^2 - 7*x2^2*x3^2)/r^4)/r^5;
f53 = f43*x2/x1;
f61 = mu*x1*x3*(3 + 7.5*temp*(3*r^2 - 7*x3^2)/r^4)/r^5;
f62 = f61*x2/x1;
f63 = -mu*(r^2 - 3*x3^2 + 4.5*temp - 7.5*temp*(6*x3^2*r^2 - 7*x3^4)/r^4)/r^5;
% F: 状态转移矩阵
F = [0 0 0 1 0 0;
     0 0 0 0 1 0;
     0 0 0 0 0 1;
     f41 f42 f43 0 0 0;
     f51 f52 f53 0 0 0;
     f61 f62 f63 0 0 0];

% phi_k = eye(6) + F*delta + F^2*delta^2/2;
fai = eye(6) + F*deta;  % 线性化状态转移矩阵

% ======================== 求解观测矩阵 ========================
xx1 = yubao_y(1);
xx2 = yubao_y(2);
xx3 = yubao_y(3);
xx4 = yubao_y(4);
xx5 = yubao_y(5);
xx6 = yubao_y(6);
rr = yubao_y(1:3);  % 位置矢量
M1=Rm(1);
M2=Rm(2);
M3=Rm(3);
S1=Rs(1);
S2=Rs(2);
S3=Rs(3);
c_alpha = dot(-rr/norm(-rr), (Rm-rr)/norm(Rm-rr));  % cos_alpha
c_beta = dot(-rr/norm(-rr), (Rs-rr)/norm(Rs-rr));   % cos_beta
h = [c_alpha; c_beta];
r1 = xx1^2 + xx2^2 + xx3^2;                 % r1, r2, r2为临时变量
r2 = (M1-xx1)^2 + (M2-xx2)^2 + (M3-xx3)^2;
r3 = xx1*(xx1-M1) + xx2*(xx2-M2) + xx3*(xx3-M3);
h11 = ((2*xx1-M1)*r1*r2 - r3*(xx1*r2 + r1*(xx1-M1)))/(r1*r2)^(3/2);
h12 = ((2*xx2-M2)*r1*r2 - r3*(xx2*r2 + r1*(xx2-M2)))/(r1*r2)^(3/2);
h13 = ((2*xx3-M3)*r1*r2 - r3*(xx3*r2 + r1*(xx3-M3)))/(r1*r2)^(3/2);
r2 = (S1-xx1)^2 + (S2-xx2)^2 + (S3-xx3)^2;        % r2, r3利了两次
r3 = xx1*(xx1-S1) + xx2*(xx2-S2) + xx3*(xx3-S3);
h21 = ((2*xx1-S1)*r1*r2 - r3*(xx1*r2 + r1*(xx1-S1)))/(r1*r2)^(3/2);
h22 = ((2*xx2-S2)*r1*r2 - r3*(xx2*r2 + r1*(xx2-S2)))/(r1*r2)^(3/2);
h23 = ((2*xx3-S3)*r1*r2 - r3*(xx3*r2 + r1*(xx3-S3)))/(r1*r2)^(3/2);
% H: 观测矩阵
H = [h11 h12 h13 0 0 0;
     h21 h22 h23 0 0 0];

% ======================================================
P_p = fai*P*fai' + Qk;	% 一步预测误差方差阵

% ??????????????????????????????????????????????????????
ff = 0.11/3*kk;	 % sin(acos(c_alpha))*sin(0.11/3*kk);
FF = 1;
Rk = FF*[ff^2 0; 0 ff^2];     % 系统观测噪声方差阵

K = P_p*H'*inv(H*P_p*H'+Rk);  % 滤波增益矩阵

% Pm = (eye(6)-K*H) * P_p * (eye(6)-K*H)' + K*Rk*K';  % 估计误差方差阵
Pm = (eye(6)-K*H) * P_p;

deta_y = (K * (Z-h))';

ym = yubao_y + deta_y;  % ????

% the end of "EKF.m".