UKFinNavigation.rar_GPS+惯导的算法_UKF 惯导_gps 惯导_惯导GPS_松组合UKF

%GPS/INS无反馈位置组合 卡尔曼滤波器 %每秒更新一次速度位置误差 %连续状态系统方程 %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_phi, E_velocity, E_position, X] = 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 陀螺仪误差漂移相关时间 【2】误差漂移相关时间？？？ %tao 迭代步长 %%%%%%%输入向量均为行向量%%%%%%%%%%%%% %输出 %E_position 位置预测值 %E-velocity 速度预测值 %各参数初始化 Re = 6378245; %地球长半径 e = 1/298.257; %地球扁率 wie = 7.292e-5; %地球自转角速度 rad_deg = 0.01745329; % 东北天速度 Ve0 = v(:,1); Vn0 = v(:,2); Vu0 = v(:,3); % 导航位置 L0 = p(:,1); h0 = p(:,3); %失准角赋值 PhiE0 = 0.5*rad_deg; %太小 PhiN0 = 0.5*rad_deg; PhiU0 = 26*rad_deg; %卡尔曼滤波参数初始化 【P0 Q R】 PP(1:15,1:15) = diag([1/(36*57) 1/(36*57) 1/57, 0.0001 0.0001 0.0001, 0 0 1, 0.04/(57*3600) 0.04/(57*3600) 0.04/(57*3600), 1e-4 1e-4 1e-4].^2); %初始误差协方差阵 PP0 = PP; P = eye(15); X = zeros(15,1); %初始状态 X(1:3) = [PhiE0;PhiN0;PhiU0]; % X(4:6) = E_phi = zeros(1,3); E_position = zeros(1,3); E_velocity = zeros(1,3); n = size(Dp,1); xV = zeros(15,n); %estmate % allocate memory 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) ]; PhiE = X(1); PhiN = X(2); PhiU = X(3); %连续系统状态转换阵 F 的时间更新 F = zeros(15,15); FN = [ 0, wie*sin(L) + (Ve*tan(L))/(Rn + h), cos(PhiU)*(Ve/(Rn + h) + wie*cos(L)) - (Vn*sin(PhiU))/(Rm + h), 0, -1/(Rm + h), 0, 0, 0, Vn/(Rm + h)^2; - wie*sin(L) - (Ve*tan(L))/(Rn + h), 0, sin(PhiU)*(Ve/(Rn + h) + wie*cos(L)) + (Vn*cos(PhiU))/(Rm + h), 1/(Rn + h), 0, 0, -wie*sin(L), 0, -Ve/(Rn + h)^2; cos(PhiU)*(Ve/(Rn + h) + wie*cos(L)) - (Vn*sin(PhiU))/(Rm + h), sin(PhiU)*(Ve/(Rn + h) + wie*cos(L)) + (Vn*cos(PhiU))/(Rm + h), (Ve/(Rn + h) + wie*cos(L))*(PhiN*cos(PhiU) - PhiE*sin(PhiU)) - (Vn*(PhiE*cos(PhiU) + PhiN*sin(PhiU)))/(Rm + h), tan(L)/(Rn + h), 0, 0, wie*cos(L) + Ve/(cos(L)^2*(Rn + h)), 0, -(Ve*tan(L))/(Rn + h)^2; 0, -fu, - fn*cos(PhiU) - fe*sin(PhiU), (Vn*tan(L))/(Rn + h) - Vu/(Rn + h), 2*wie*sin(L) + (Ve*tan(L))/(Rn + h), - Ve/(Rn + h) - 2*wie*cos(L), Vn*(2*wie*cos(L) + Ve/(cos(L)^2*(Rn + h))) + 2*Vu*wie*sin(L), 0, (Ve*Vu)/(Rn + h)^2 - (Ve*Vn*tan(L))/(Rn + h)^2; fu, 0, fe*cos(PhiU) - fn*sin(PhiU), - 2*wie*sin(L) - (2*Ve*tan(L))/(Rn + h), -Vu/(Rm + h), -Vn/(Rm + h), -Ve*(2*wie*cos(L) + Ve/(cos(L)^2*(Rn + h))), 0, (Ve^2*tan(L))/(Rn + h)^2 + (Vn*Vu)/(Rm + h)^2; - fn*cos(PhiU) - fe*sin(PhiU), fe*cos(PhiU) - fn*sin(PhiU), - fe*(PhiE*cos(PhiU) + PhiN*sin(PhiU)) - fn*(PhiN*cos(PhiU) - PhiE*sin(PhiU)), (2*Ve)/(Rn + h) + 2*wie*cos(L), (2*Vn)/(Rm + h), 0, -2*Ve*wie*sin(L), 0, - Ve^2/(Rn + h)^2 - Vn^2/(Rm + h)^2; 0, 0, 0, 0, 1/(Rm + h), 0, 0, 0, -Vn/(Rm + h)^2; 0, 0, 0, 1/(cos(L)*(Rn + h)), 0, 0, (Ve*tan(L))/(cos(L)*(Rn + h)), 0, -Ve/(cos(L)*(Rn + h)^2); 0, 0, 0, 0, 0, 1, 0, 0, 0 ] ; F(1:9, 1:9) = FN; F(1:3,10:12) = Cnb; F(4:6,13:15) = Cnb; F(10,10) = -1/Tg(1); F(11,11) = -1/Tg(2); F(12,12) = -1/Tg(3); F(13,13) = -1/Ta(1); F(14,14) = -1/Ta(2); F(15,15) = -1/Ta(3); %连续系统输入矩阵更新 G = zeros(15,6); G(1:3,1:3) = Cnb; G(4:6,4:6) = Cnb; G(10:12,1:3) = eye(3,3); G(13:15,4:6) = eye(3,3); %连续系统量测阵更新 H = zeros(3,15); H(1,7) = 1; H(2,8) = 1; H(3,9) = 1; %连续系统离散化 A = eye(15,15)+F*tao; B = (eye(15,15)+tao*F/2)*G*tao; const = [wie L Rm Rn Ve Vn Vu fe fn fu h]; % f = @(X)F_X(const, X); f = @(X)A*X; z = @(X)H*X; % %卡尔曼滤波 % P = A*(PP0)*A'+ B*Q*B'; % K = P*H'*inv(H*P*H'+ R); % PP0 = (eye(15,15) - K*H)*P; % PP0 = (PP0+PP0')/2; % PP(i,:) = diag(PP0); % % z = Dp(i+1,:)'; % % Xklk_1 = F_X(const, X); % % XX = Xklk_1 + K*(z - H*Xklk_1); % % XX = A*X+K*(z-H*A*X); [X, P] = ukf(f,X,P,z,Dp(i+1,:)',Q,R); % ukf % P xV(:,i) = X; % save estimate XX = X; E_phi(i,:) = XX(1:3)'; E_velocity(i+1,:) = XX(4:6)'; E_position(i+1,:) = XX(7:9)'; end % 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)