zw_INGPS.zip

zw_INGPS.zip

clear all;
 close all;
clc;

load（‘E:\实验室资料\学习资料\研一下学习资料\卡尔曼滤波与组合导航\kalman实验报告__邹莹\实验三\数据\IMU.dat‘）
load（‘E:\实验室资料\学习资料\研一下学习资料\卡尔曼滤波与组合导航\kalman实验报告__邹莹\实验三\数据\GPS.dat‘）
% Pi = 3.1415926535897931;
% WIEE = 0.000072921151467;
% Re = 6378135.072;
% g0 = 9.7803267714;
% e = 1.0 / 298.25;
% T_IMU = 0.01;      %%导航数据时间间隔
% T_GPS = 0.05;      %%GPS数据时间间隔
% LOOP = 360000;     %%循环次数，数据长度
% 
% 
% velocity_GPSINS（72003） = 0;
% position_GPSINS（72003） = 0;
% attitude（LOOP3） = 0;
% x_kf（LOOP/515） = 0;
% p_kf（LOOP/515） = 0;
% 
% %导航参数初始化赋值
% Lat = GPS（13）*Pi/180;
% Lon = GPS（14）*Pi/180;
% Hei = GPS（15）;
% Vx = GPS（16）;
% Vy = GPS（17）;
% Vz = GPS（18）;
% 
% IMU（1:3600013:5） = IMU（1:3600013:5）*Pi/180/3600;
% IMU（1:3600016:8） = IMU（1:3600016:8）*g0;
% %姿态角
% fai = 305.34023*Pi/180;
% theta = 0.25097*Pi/180;
% gama = 1.78357*Pi/180;
% 
% %初始方向余弦矩阵和四元数
% Cnb = [  cos（gama）*cos（fai）-sin（gama）*sin（theta）*sin（fai） cos（gama）*sin（fai）+sin（gama）*sin（theta）*cos（fai） -sin（gama）*cos（theta）;	
%          -cos（theta）*sin（fai） cos（theta）*cos（fai） sin（theta）;
%          sin（gama）*cos（fai）+cos（gama）*sin（theta）*sin（fai） sin（gama）*sin（fai）-cos（gama）*sin（theta）*cos（fai） cos（gama） * cos（theta）];
% q = [ cos（fai/2）*cos（theta/2）*cos（gama/2） - sin（fai/2）*sin（theta/2）*sin（gama/2）;
%       cos（fai/2）*sin（theta/2）*cos（gama/2） - sin（fai/2）*cos（theta/2）*sin（gama/2）;
%       cos（fai/2）*cos（theta/2）*sin（gama/2） + sin（fai/2）*sin（theta/2）*cos（gama/2）;
%       cos（fai/2）*sin（theta/2）*sin（gama/2） + sin（fai/2）*cos（theta/2）*cos（gama/2）];
%   
% Kf_loop = 0;
% Q = diag（[（0.2*Pi/180/3600）^2（0.2*Pi/180/3600）^2（0.2*Pi/180/3600）^2（100E-6*g0）^2（100E-6*g0）^2（100E-6*g0）^2]）;
% R = diag（[0.01^20.01^20.01^20.1^20.1^20.15^2]）;
% p_filter = diag（[（0.5*Pi/180）^2（1*Pi/180/60）^2（1*Pi/180/60）^2 0.05^20.05^20.05^2 （2/Re）^2（2/Re）^22^2... 
%                  （0.1*Pi/180/3600）^2（0.1*Pi/180/3600）^2（0.1*Pi/180/3600）^2（50E-6*g0）^2（50E-6*g0）^2（50E-6*g0）^2]）;
% x_filter = zeros（151）;
%   
% for i= 1:360000
%     Rmh = Re * （1 - 2 * e + 3 * e * sin（Lat） * sin（Lat）） + Hei;
%     Rnh = Re * （1 + e * sin（Lat） * sin（Lat）） + Hei;
%     g_s = g0 * （1 + 0.00193185138639 * sin（Lat） * sin（Lat）） / sqrt（1 - 0.00669437999013 * sin（Lat） * sin（Lat）） * （1 - 2 * Hei / Re）;
%     Wien = [ 0; WIEE * cos（Lat）; WIEE * sin（Lat）];			
%     Wenn = [ -Vy / Rmh; Vx / Rnh; Vx * tan（Lat） / Rnh];
%     Winn = Wien + Wenn;
%     Winb = Cnb * Winn;
%     Wibb = IMU（i3:5）‘;
%     Wnbb = Wibb - Winb;
%     angle = Wnbb * T_IMU;         %%每次姿态变化对应的小角度变化，用于姿态更新
%     
%     
% % edit by STX       
% % fn = Cnb‘ * IMU（i6:8）‘;      %%比力在n系中的投影
% % V=[Vx;Vy;Vz];
% % FN=[fn（1）;fn（2）;fn（3）];
% % Wx_temp=2*Wien+Wenn;
% % g=[0;0;g_s];
% % difV=FN-cross（Wx_tempV）-g;
% % difVx=difV（1）;
% % difVy=difV（2）;
% % difVz=difV（3）;
% % Vx=V（1）;Vy=V（2）;Vz=V（3）;
% 
%     
%      fn = Cnb‘ * IMU（i6:8）‘;      %%比力在n系中的投影
%     difVx = fn（1） + （2 * WIEE

 属性            大小     日期    时间   名称
----------- ---------  ---------- -----  ----
     文件       27612  2019-03-08 22:21  \INGPS.m
     文件          36  2019-03-08 22:21  \no.txt