组合导航程序

clear
clc
close all
%%初始量定义
wie = 0.000072921151467; 
Re	= 6378135.072;
g = 9.7803267714;
e = 1.0 / 298.25;
T = 0.01;                      %IMU频率100hz此程序中GPS频率100hz
datanumber = 72001;           %数据时间3600s                  

a = load（‘C:\Users\wangbo\Desktop\2学习\程序\IMU.dat‘）;
w = a（:3:5）‘*pi/180/3600;    %陀螺仪输出的角速率信息单位由°/h化为rad/s
f = a（:6:8）‘;                %三轴比力输出，单位g
a = load（‘C:\Users\wangbo\Desktop\2学习\程序\GPS.dat‘）;    
gps_pos = a（:3:5）;                       %GPS输出的纬度、经度、高度信息
gps_pos（:1:2） = gps_pos（:1:2）*pi/180;   %纬 经单位化为弧度
gps_v = a（:6:8）;                         %GPS输出的东北天速度信息

%捷联解算及卡尔曼相关
v=zeros（datanumber3）;                    %组合后的速度信息ffffffff
atti = zeros（datanumber3）;               %组合后的姿态信息
pos = zeros（datanumber3）;                %组合后的位置信息

gyro=zeros（31）;
acc=zeros（31）;

x_kf = zeros（datanumber15）; 
p_kf = zeros（datanumber15）;

lat = 40.0211142228246*pi/180;             %组合导航的初始位置、姿态、速度
lon =116.3703629769560*pi/180;
height =43.0674;
fai = 219.9744642380873*pi/180;
theta = -0.895865732956914*pi/180;
gama = 0.640089448357591*pi/180;
Vx=gps_v（11）;
Vy=gps_v（12）;
Vz=gps_v（13）;

X_o=zeros（151）;    %X的初值选为0
X=zeros（151）;
 
%Q=diag（[（50e-6*g）^2（50e-6*g）^2（50e-6*g）^2（0.1*pi/180/3600）^2（0.1*pi/180/3600）^2（0.1*pi/180/3600）^2000000000]）;     %随机
Q=diag（[（0.008*pi/180/3600）^2（0.008*pi/180/3600）^2（0.008*pi/180/3600）^2（50e-6*g）^2（50e-6*g）^2（50e-6*g）^2000000000]）;
R=diag（[（0.001）^2（0.001）^2（0.001）^2（0.1）^2（0.1）^2（0.15）^2]）;       
P=zeros（15）;
P_k=diag（[（0.00005*pi/180）^2（0.00005*pi/180）^2（0.00005*pi/180）^20.00005^20.00005^20.00005^22^22^22^2（0.001*pi/180/3600）^2（0.001*pi/180/3600）^2（0.001*pi/180/3600）^2（50e-6*g）^2（50e-6*g）^2（50e-6*g）^2]）;      %
K=zeros（156）;
Z=zeros（61）;
I=eye（15）;

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）];

Cnb_s=Cnb;
q_s=q;
  
  
for i=1:1:datanumber
       
    Rmh = Re * （1.0 - 2.0 * e + 3.0 * e * sin（lat） * sin（lat）） + height;
    Rnh = Re * （1.0 + e * sin（lat） * sin（lat）） + height;
    
    Wien = [ 0; wie * cos（lat）; wie * sin（lat）];			
    Wenn = [ -Vy / Rmh; Vx / Rnh; Vx * tan（lat） / Rnh];
    Winn = Wien + Wenn;
    Winb = Cnb * Winn;
    
    for j=1:3
        gyro（j1） = w（ji）;
        acc（j1） = f（ji）*g;     %加速度信息，单位化为m/s^2
    end
    
    angle = （gyro - Winb）