惯导轨迹生成

close all;
clear all;
clc;
format long;
fid = fopen（‘C:\Documents and Settings\Administrator\桌面\imusimulate\dataagj.txt‘‘w‘）;
%% 地球参数
global GLV;
GLV.RE          = 6378137; % length of the semi-major aixs unit: m
GLV.RP          = 6356752; % length of the semi-minor aixs unit: m
GLV.R           = sqrt（GLV.RE*GLV.RP）; % length of the approximate semidiameter
GLV.F           = 1/298.257223563; % flattening of the ellipsoid
GLV.E           = 0.081819191042816; % major eccentricity of the ellipsoid
GLV.OMEGA_IE    = 7.2921158e-5; % earth‘s rate unit: rad/s
GLV.G0          = 9.78031811; % gravity coefficient unit: m/s^2
GLV.G           = 9.8010104; % approximate gravity unit: m/s^2
%% 常数
TTR = pi/180;%%角度值转为弧度值
%% 解算周期
deltt = 5e-3;%%%5ms
period = 5e-3;%%%5ms
%% 系统运行时间
t = 200*60;%% 200min
calc_len = t/deltt;%%系统计算数据长度
% calc_len = 10;
%% 航迹初始值设定
%%% atti = [roll; pitch; heading];姿态角矩阵
atti_0 = [0.4;0.4;3]*TTR/60;%%姿态初始值
datti = [0;0;0];%%姿态变化率
%%% vn = [vE;vN;vU];地理坐标系n系中的速率分量
vn = [0;1.5;0];%%轨迹生成中假定载体由南向北匀速运动
%%% posn = [latlonhigh];地理坐标系n系中的位置变量由球面坐标表示
posn_0 = [39.229956; 117.143156; 0]*TTR;
% posn = zeros（3calc_len）;%%%位置矩阵
% atti = zeros（3calc_len）;%%%姿态角矩阵
% wibb = zeros（3calc_len）;%%%陀螺数据
% fb =zeros（3calc_len）;%%%加速度数据

 %%% 元件参数
  %%% 陀螺
    gyrobias_err = 0.01*pi/180/3600;
    gyrobias_var = 0.01*pi/180/3600;
    gyrosca_err = 1e-5;
  %%% 加表
    accbias_err = 1e-4*GLV.G;
    accbias_var = 5e-5*GLV.G;
    nosie_g = ones（31）*（gyrobias_err+gyrobias_var.^（0.5）*randn（1calc_len））;
    noise_a = ones（31）*（accbias_err+accbias_var.^（0.5）*randn（1calc_len））;
    
posn（:1） = posn_0;
atti（:1） = atti_0;

for i=1:calc_len
   lat = posn（1i）; lon = posn（2i）; high = posn（3i）;
   sinL = sin（lat）; cosL = cos（lat）; tanL = tan（lat）;

   g = GLV.G0*（1+5.3024e-3*sinL^2-5.9e-6*（sin（2*lat））^2）;
   RM = GLV.RE*（1-GLV.E^2）/（（1-（GL