%GPS/INS无反馈位置组合 卡尔曼滤波器
%%%%%%%%%%%%%%%%%%
%edit by horsejun
%%%%%%%%%%%%%%%%%%
%每秒更新一次速度位置误差
%连续状态系统方程
%dx = F*x + G*w
%z = H*x + v
%离散状态系统方程
%x(k+1) = A*x(k) + B*w(k)
%z(k+1) = C*x(k+1) + v(k+1)
function [E_attitude, E_velocity, E_position, PP] = kalman_GPS_INS_position_sp_NFb(Dp, v, p, quat, Fn, Q, R, Tg, Ta, tao)
%输入
%Dp 量测位置误差， 作为滤波器输入,
%Dv 量测速度误差， 作为滤波器输入，
%p ins输出位置，作为滤波器系统参数
%v ins输出速度，作为滤波器系统参数
%fn ins输出导航系下比力，作为滤波器参数
%quat ins输出四元数，作为滤波器参数
%Q 系统噪声方差
%R 测量噪声方差
%Ta 加表误差漂移相关时间
%Tg 陀螺仪误差漂移相关时间
%tao 迭代步长
%%%%%%%输入向量均为行向量%%%%%%%%%%%%%
%输出
%E_position 位置预测值
%E-velocity 速度预测值
%各参数初始化
Re = 6378245; %地球长半径
e = 1/298.257; %地球扁率
wie = 7.292e-5; %地球自转角速度
% 东北天速度
Ve0 = v(:,1);
Vn0 = v(:,2);
Vu0 = v(:,3);
% 导航位置
L0 = p(:,1);
h0 = p(:,3);
%卡尔曼滤波参数初始化
PP(1:18,1:18) = diag([1/(36*57) 1/(36*57) 1/57, 0.0001 0.0001 0.0001, 0 0 1, 0.1/(57*3600) 0.1/(57*3600) 0.1/(57*3600), 0.04/(57*3600) 0.04/(57*3600) 0.04/(57*3600), 1e-4 1e-4 1e-4].^2); %初始误差协方差阵
PP0 = PP;
X = zeros(18,1); %初始状态
E_attitude = zeros(1,3);
E_position = zeros(1,3);
E_velocity = zeros(1,3);
n = size(Dp,1);
for i=1:n-1
%参数赋值
Ve = Ve0(i);
Vn = Vn0(i);
Vu = Vu0(i);
L = L0(i);
h = h0(i);
fe = Fn(i,1);
fn = Fn(i,2);
fu = Fn(i,3);
Rm = Re*(1-2*e+3*e*sin(L)^2);
Rn = Re*(1-e*sin(L)^2);
%由四元数计算姿态阵
q = quat(i,:);
Cnb = [1-2*(q(3)^2+q(4)^2), 2*(q(2)*q(3)-q(1)*q(4)), 2*(q(2)*q(4)+q(1)*q(3));
2*(q(2)*q(3)+q(1)*q(4)), 1-2*(q(2)^2+q(4)^2), 2*(q(3)*q(4)-q(1)*q(2));
2*(q(2)*q(4)-q(1)*q(3)), 2*(q(3)*q(4)+q(1)*q(2)), 1-2*(q(2)^2+q(3)^2)];
%连续系统状态转换阵 F 的时间更新
F = zeros(18,18);
F(1,2) = wie*sin(L)+Ve*tan(L)/(Rn+h);
F(1,3) = -(wie*cos(L)+Ve/(Rn+h));
F(1,5) = -1/(Rm+h);
F(1,9) = Vn/(Rm+h)^2;
F(2,1) = -(wie*sin(L)+Ve*tan(L)/(Rn+h));
F(2,3) = -Vn/(Rm+h);
F(2,4) = 1/(Rn+h);
F(2,7) = -wie*sin(L);
F(2,9) = -Ve/(Rn+h)^2;
F(3,1) = wie*cos(L)+Ve/(Rn+h);
F(3,2) = Vn/(Rm+h);
F(3,4) = tan(L)/(Rn+h);
F(3,7) = wie*cos(L)+Ve*(sec(L)^2)/(Rn+h);
F(3,9) = -Ve*tan(L)/(Rn+h)^2;
F(4,2) = -fu;
F(4,3) = fn;
F(4,4) = Vn*tan(L)/(Rm+h)-Vu/(Rm+h);
F(4,5) = 2*wie*sin(L)+Ve*tan(L)/(Rn+h);
F(4,6) = -(2*wie*cos(L)+Ve/(Rn+h));
F(4,7) = 2*wie*cos(L)*Vn+Ve*Vn*sec(L)^2/(Rn+h)+2*wie*sin(L)*Vu;
F(4,9) = (Ve*Vu-Ve*Vn*tan(L))/(Rn+h)^2;
F(5,1) = fu;
F(5,3) = -fe;
F(5,4) = -2*(wie*sin(L)+Ve*tan(L)/(Rn+h));
F(5,5) = -Vu/(Rm+h);
F(5,6) = -Vn/(Rm+h);
F(5,7) = -(2*wie*cos(L)+Ve*(sec(L)^2)/(Rn+h))*Ve;
F(5,9) = (Ve^2*tan(L)+Vn*Vu)/(Rn+h)^2;
F(6,1) = -fn;
F(6,2) = fe;
F(6,4) = 2*(wie*cos(L)+Ve/(Rn+h));
F(6,5) = 2*Vn/(Rm+h);
F(6,7) = -2*Ve*wie*sin(L);
F(6,9) = -(Vn^2+Ve^2)/(Rn+h)^2;
F(7,5) = 1/(Rm+h);
F(7,9) = -Vn/(Rm+h)^2;
F(8,4) = 1/((Rn+h)*cos(L));
F(8,7) = Ve*tan(L)/((Rn+h)*cos(L));
F(8,9) = -Ve/(cos(L)*(Rn+h)^2);
F(9,6) = 1;
F(1:3,10:12) = Cnb;
F(1:3,13:15) = Cnb;
F(4:6,16:18) = Cnb;
F(13,13) = -1/Tg(1);
F(14,14) = -1/Tg(2);
F(15,15) = -1/Tg(3);
F(16,16) = -1/Ta(1);
F(17,17) = -1/Ta(2);
F(18,18) = -1/Ta(3);
%连续系统输入矩阵更新
G = zeros(18,9);
G(1:3,1:3) = Cnb;
G(13:15,4:6) = eye(3,3);
G(16:18,7:9) = eye(3,3);
%连续系统量测阵更新
H = zeros(3,18);
H(1,7) = 1;
H(2,8) = 1;
H(3,9) = 1;
%连续系统离散化
A = eye(18,18)+F*tao;
B = (eye(18,18)+tao*F/2)*G*tao;
%卡尔曼滤波
P = A*(PP0)*A'+B*Q*B';
K = P*H'*inv(H*P*H'+R);
PP0 = (eye(18,18)-K*H)*P;
PP0 = (PP0+PP0')/2;
PP(i,:) = diag(PP0);
z = Dp(i+1,:)';
XX = A*X+K*(z-H*A*X);
X = XX;
E_attitude(i+1,:) = XX(1:3)';
E_velocity(i+1,:) = XX(4:6)';
E_position(i+1,:) = XX(7:9)';
end
