捷联惯导算法SINS.m

 clear all; 
clc;
close all;

%% 初始参数设置
g0 = 9.78049;
rad_deg = 0.01745329;%%度到弧度
wie = 7.27220417e-5;
Re = 6378393.0;
%% The duration of simulation
time = 8*60*60; %仿真时长
Hn = 1;%时间间隔
%%%陀螺常值漂移
G_Drift=[2*rad_deg/3600;   
         2*rad_deg/3600;  
        2*rad_deg/3600];
%%%陀螺随机漂移
Drift_x = 0.01*rad_deg/3600;
Drift_y = 0.01*rad_deg/3600;
Drift_z = 0.01*rad_deg/3600;

%%%%加速度计零偏
A_Bias=[1e-4*g0     
       1e-4*g0    
        1e-4*g0];

A_randn_Bias=[1e-5*g0
              1e-5*g0
              1e-5*g0];
%%%姿态角摇摆周期
T_p = 10;
T_r = 10;
T_y = 10;
%%%姿态角摇摆幅值
% pitchm = 5*rad_deg;
% rollm = 5*rad_deg;
% yawm = 10*rad_deg;

pitchm = 0*rad_deg;
rollm = 0*rad_deg;
yawm = 0*rad_deg;
%%%初始姿态角
pitchk = 0*rad_deg;
rollk = 0*rad_deg;
yawk = 0*rad_deg;
%% The initial angular phase
p_pitch = 0*rad_deg;
p_roll = 0*rad_deg;
p_yaw = 0*rad_deg;

pitch_error=1*rad_deg;
roll_error=1*rad_deg;
yaw_error=1*rad_deg;%%%%%%%初始平台失准角（粗对准以后的结果）

%% The true initial position parameters used for simulator
lati = 45.7796*rad_deg;
longi = 126.6705*rad_deg;
wien = [0
        wie*cos（lati）
        wie*sin（lati）];
%% These parameters used for SINS updating calculation 
phi=45.7796*rad_deg;
lamda=126.6705*rad_deg;
wiep=[0
      wie*cos（phi）
      wie*sin（phi）];

pitch0 = pitchm*sin（p_pitch）+pitchk;
roll0 = rollm*sin（p_roll）+rollk;
yaw0 = yawm*sin（p_yaw）+yawk;
%%%运动加速度
aold = 0.0*g0;
a = [ aold; aold; 0.0];

%% The initial velocity
% v0=[10.0*sin（yawk）;10.0*cos（yawk）;0.0];
% v0=[10.0;10.0;0.0];
v0 = [0.0;0.0;0.0];
v = v0;

position_error1 = 0;
position_error2 = 0;

wepp=[-v（2）/Re
       v（1）/Re
       v（1）*tan（phi）/Re];
   
q=[1  0   0   0]‘;

%% Establish the initial strapdown matrix with initial attitude angles
pitch00 = pitch0;
roll00 = roll0;
yaw00 = yaw0;

Tbn（11） = cos（roll00）*cos（yaw00） - sin（roll00）*sin（pitch00）*sin（yaw00）;
Tbn（12） = -cos（pitch00）*sin（yaw00）;
Tbn（13） = sin（roll00）*cos（yaw00） + cos（roll00）*sin（pitch00）*sin（yaw00）;

Tbn（21） =  cos（roll00）*sin（yaw00） + sin（roll00）*sin（pitch00）*cos（yaw00）;
Tbn（22） = cos（pitch00）*cos（yaw00） ;
Tbn（23） =  sin（roll00）*sin（yaw00） - cos（roll00）*sin（pitch00）*cos（yaw00）;

Tbn（31） = -sin（roll00）*cos（pitch00）;
Tbn（32） = sin（pitch00）;
Tbn（33） = cos（roll00）*cos（pitch00）;

TT=[1          yaw_error     -roll_error;
    -yaw_error 1             pitch_error;
    roll_error -pitch_error  1];
T = TT*Tbn;
 
q（1） = sqrt（1+T（11）+T（22）+T（33））/2.0;
q（2） = （T（32）-T（23））/（4*q（1））;
q（3） = （T（13）-T（31））/（4*q（1））;
q（4） = （T（21）-T（12））/（4*q（1））;

Tn = time/Hn;
%%***** 轨迹发生器 *****%%模拟惯导输出
for k = 1:Tn

    for i=1:3
        pitchT（i） = pitchm*sin（2*pi*（k-（3-i）/2）*Hn/T_p+p_pitch）+pitchk;
        rollT（i）  = rollm*sin（2*pi*（k-（3-i）/2）*Hn/T_r+p_roll）+rollk;
        yawT（i）   = yawm*sin（2*pi*（k-（3-i）/2）*Hn/T_y+p_yaw）+yawk;
        
        wpT（i） = 2*pi/T_p*pitchm*cos（2*pi*（k-（3-i）/2）*Hn/T_p+p_pitch）;
        wrT（i） = 2*p