旋转椭球体的4个基本参数：长半轴、扁率(椭圆度)、地心引力常数、自转角速率；
以上内容来自：《车载定位定向系统关键技术研究》付强文
旋转椭球体：
地球自转角速度：
地球重力加速度为：
子午圈和卯酉圈曲率半径为：
以上内容来自：《捷联惯导算法及车载组合导航系统研究（硕士论文）》严恭敏
global glv
if ~exist('Re', 'var'), Re = []; end
if ~exist('f', 'var'), f = []; end
if ~exist('wie', 'var'), wie = []; end
if isempty(Re), Re = 6378137; end
if isempty(f), f = 1/298.257; end
if isempty(wie), wie = 7.2921151467e-5; end
glv.Re = Re; % the Earth's semi-major axis
glv.f = f; % flattening
glv.Rp = (1-glv.f)*glv.Re; % semi-minor axis
glv.e = sqrt(2*glv.f-glv.f^2); glv.e2 = glv.e^2; % 1st eccentricity
glv.ep = sqrt(glv.Re^2-glv.Rp^2)/glv.Rp; glv.ep2 = glv.ep^2; % 2nd eccentricity
glv.wie = wie; % the Earth's angular rate
glv.meru = glv.wie/1000; % milli earth rate unit
glv.g0 = 9.7803267714; % gravitational force
glv.mg = 1.0e-3*glv.g0; % milli g
glv.ug = 1.0e-6*glv.g0; % micro g 为 微g
glv.mGal = 1.0e-3*0.01; % milli Gal = 1cm/s^2 ~= 1.0E-6*g0
glv.ugpg2 = glv.ug/glv.g0^2; % ug/g^2
glv.ws = 1/sqrt(glv.Re/glv.g0); % Schuler frequency
glv.ppm = 1.0e-6; % parts per million百万分之一
glv.deg = pi/180; % arcdeg 弧度单位 rad
glv.min = glv.deg/60; % arcmin
glv.sec = glv.min/60; % arcsec
glv.hur = 3600; % time hour (1hur=3600second)
glv.dps = pi/180/1; % arcdeg / second
glv.dph = glv.deg/glv.hur; % arcdeg / hour ;最终单位为 rad/s;
%比如：imuerr.eb(1:3) = 1*glv.dph；即为：((1*pi/180)/3600)rad/s
glv.dpss = glv.deg/sqrt(1); % arcdeg / sqrt(second) 为 rad/sqrt(s)
glv.dpsh = glv.deg/sqrt(glv.hur); % arcdeg / sqrt(hour) 为 rad/sqrt(hour);最终单位为：rad/sqrt(s)
%imuerr.web(1:3) = web*glv.dpsh；即为：((1*pi/180)/sqrt(3600))rad/s
glv.dphpsh = glv.dph/sqrt(glv.hur); % (arcdeg/hour) / sqrt(hour) 为 rad/hour/sqrt(hour)
glv.Hz = 1/1; % Hertz
glv.dphpsHz = glv.dph/glv.Hz; % (arcdeg/hour) / sqrt(Hz) 为 rad/hour/sqrt(Hz)
glv.ugpsHz = glv.ug/sqrt(glv.Hz); % ug / sqrt(Hz)
glv.ugpsh = glv.ug/sqrt(glv.hur); % ug / sqrt(hour)
glv.mpsh = 1/sqrt(glv.hur); % m / sqrt(hour)
glv.mpspsh = 1/1/sqrt(glv.hur); % (m/s) / sqrt(hour), 1*mpspsh~=1700*ugpsHz
glv.ppmpsh = glv.ppm/sqrt(glv.hur); % ppm / sqrt(hour)
glv.mil = 2*pi/6000; % mil
glv.nm = 1853; % nautical mile 海里
glv.kn = glv.nm/glv.hur; % knot 节
%%
glv.wm_1 = [0,0,0]; glv.vm_1 = [0,0,0]; % the init of previous gyro & acc sample
glv.cs = [ % coning & sculling compensation coefficients
[2, 0, 0, 0, 0 ]/3
[9, 27, 0, 0, 0 ]/20
[54, 92, 214, 0, 0 ]/105
[250, 525, 650, 1375, 0 ]/504
[2315, 4558, 7296, 7834, 15797]/4620 ];
glv.csmax = size(glv.cs,1)+1; % max subsample number
glv.v0 = [0;0;0]; % 3x1 zero-vector
glv.qI = [1;0;0;0]; % identity quaternion 初始四元数
glv.I33 = eye(3); glv.o33 = zeros(3); % identity & zero 3x3 matrices
glv.pos0 = [34.246048*glv.deg; 108.909664*glv.deg; 380]; % position of INS Lab@NWPU
glv.eth = []; glv.eth = earth(glv.pos0);
%%
[glv.rootpath, glv.datapath, glv.mytestflag] = psinsenvi;
glv1 = glv;俯仰角抬头为正；横滚角右倾斜为正；
其中方位角变化时（转弯时），先将飞机滚转相应的角度；即：先滚转、后转弯；
已知航向角速率、前向速度，利用向量叉乘得到向心力；利用如下公式，计算出滚转角、滚转角速率、
代码实现：
%‘2’为航向角速率(转弯)，2deg/s，转弯时间为45秒;
%协调转弯时间为4秒，即飞机横滚时间，很滚角速率 根据前向速度和航向角速率计算出
seg = trjsegment(seg, 'coturnleft', 45, 2, xxx, 4);
trjsegment子函数中对应部分：
case 'coturnleft', % coordinate turn left
rolllasting = var1; rollw = atan(cf/9.8)/dps/rolllasting; %rollw为横滚角速率（deg/s）
seg = trjsegment(seg, 'rollleft', rolllasting, rollw);
seg = trjsegment(seg, 'turnleft', lasting, w);
seg = trjsegment(seg, 'rollright', rolllasting, rollw);
case 'turnleft',
seg.wat = [seg.wat; [lasting, seg.vel, 0, 0, w*dps,-cf, 0, 0]];
case 'turnright',
seg.wat = [seg.wat; [lasting, seg.vel, 0, 0,-w*dps, cf, 0, 0]];
case 'rollleft',
seg.wat = [seg.wat; [lasting, seg.vel, 0,-w*dps, 0, 0, 0, 0]];
case 'rollright',
seg.wat = [seg.wat; [lasting, seg.vel, 0, w*dps, 0, 0, 0, 0]];描述一段完整的运动轨迹，其中初始姿态为0、初始速度为0、初始位置为【34.246048; 108.909664; 380】
通过轨迹可以看出，最终：roll=pitch=0；yaw=0°;飞机朝北飞行；飞机速度矢量为0矢量；轨迹仿真时间为：966秒
航向角：
水平姿态角：
存在问题地方：生成角增量和速度增量的公式：
由于纯惯导解算高度通道发散；
1、IMU误差生成函数和IMU数据添加误差函数：imuerrset.m和imuadderr.m
此处主要添加两项误差：常值零偏和随机游走为例：
（1）imuerrset.m
%% constant bias & random walk
imuerr.eb(1:3) = eb*glv.dph; imuerr.web(1:3) = web*glv.dpsh;
imuerr.db(1:3) = db*glv.ug; imuerr.wdb(1:3) = wdb*glv.ugpsHz;最终：
imuerr.eb单位为：rad/s ; imuerr.web单位为：rad/sqrt(s)
imuerr.db单位为：m/s^2 ; imuerr.wdb单位为：m/s^2/sqrt(s)
（2）imuadderr.m
drift = [ ts*imuerr.eb(1) + sts*imuerr.web(1)*randn(m,1), ...
ts*imuerr.eb(2) + sts*imuerr.web(2)*randn(m,1), ...
ts*imuerr.eb(3) + sts*imuerr.web(3)*randn(m,1), ...
ts*imuerr.db(1) + sts*imuerr.wdb(1)*randn(m,1), ...
ts*imuerr.db(2) + sts*imuerr.wdb(2)*randn(m,1), ...
ts*imuerr.db(3) + sts*imuerr.wdb(3)*randn(m,1) ];
imu(:,1:6) = imu(:,1:6) + drift;2、初始姿态、速度、位置误差
avpseterr.m
function davp = avpseterr(phi, dvn, dpos)
% avp errors setting.
% Inputs: phi - platform misalignment angles. NOTE: leveling errors
% phi(1:2) in arcsec, azimuthe error phi(3) in arcmin
% dvn - velocity errors in m/s
% dpos - position errors dpos=[dlat;dlon;dhgt], all in m
% Output: davp = [phi; dvn; dpos]3、利用一阶马尔可夫生成气压高度计仿真信息
bhsimu.m
t = (trj.avp(1,10):ts:trj.avp(end,10))';
bh = interp1(trj.avp(:,10), trj.avp(:,9), t, 'linear');
bh = bh + bias + markov1(var, tau, ts, length(bh),1);一阶马尔可夫过程生成的数据为：
利用一阶马尔可夫+初始高度偏差+真实高度=气压高度计，结果为;
目的是每次捷联惯导更新时，都将气压高度计送到位置信息中；
ins.vn(3) = href(k1,2); ins.pos(3) = href(k1,1);4、纯惯导解算：
（1）首先对地球、载体相关参数进行外推以双子样为例，nts=2*ts，外推ts;
%% earth & angular rate updating
vn01 = ins.vn+ins.an*nts2; pos01 = ins.pos+ins.Mpv*vn01*nts2; % extrapolation at t1/2
ins.eth = ethupdate(ins.eth, pos01, vn01);
ins.wib = phim/nts; ins.fb = dvbm/nts; % same as trjsimu
ins.web = ins.wib - ins.Cnb'*ins.eth.wnie;
% ins.wnb = ins.wib - ins.Cnb'*ins.eth.wnin;
ins.wnb = ins.wib - (ins.Cnb*rv2m(phim/2))'*ins.eth.wnin; % 2014-11-30 因为是外推1/2时刻，所以除以2主要是利用：
（2）速度更新：以双子样为例，nts=2*ts
程序中，首先根据子样数进行圆锥补偿，得到dvbm，即速度增量；
nn = size(imu,1);
nts = nn*ins.ts; nts2 = nts/2; ins.nts = nts;
[phim, dvbm] = cnscl(imu,0); % coning & sculling compensation
% [phim, dvbm] = cnscl0(imu); % coning & sculling compensation
phim = ins.Kg*phim-ins.eb*nts; dvbm = ins.Ka*dvbm-ins.db*nts; % calibration然后根据速度更新公式：
其中速度更新公式中的一项如下：dvbm/Δt就等于括号中的内容，也就是下面的 ins.fb
也就是ins.fn=qmulv(ins.qnb,ins.fb);然后通过旋转矢量进行变换；
%% (1)velocity updating
ins.fn = qmulv(ins.qnb, ins.fb);
% ins.an = qmulv(rv2q(-ins.eth.wnin*nts2),ins.fn) + ins.eth.gcc;
ins.an = rotv(-ins.eth.wnin*nts2, ins.fn) + ins.eth.gcc;
vn1 = ins.vn + ins.an*nts;（3）位置更新：以双子样为例，nts=2*ts
利用上面外推nts时刻得到的地球参数和速度更新得到的速度；利用梯形积分得到位置；
%% (2)position updating
% ins.Mpv = [0, 1/ins.eth.RMh, 0; 1/ins.eth.clRNh, 0, 0; 0, 0, 1];
ins.Mpv(4)=1/ins.eth.RMh; ins.Mpv(2)=1/ins.eth.clRNh;
% ins.Mpvvn = ins.Mpv*((ins.vn+vn1)/2+(ins.an-ins.an0)*nts^2/3); % 2014-11-30
ins.Mpvvn = ins.Mpv*(ins.vn+vn1)/2;
ins.pos = ins.pos + ins.Mpvvn*nts;
ins.vn = vn1;
ins.an0 = ins.an;（4）姿态更新：以双子样为例，nts=2*ts
%% (3)attitude updating
ins.Cnb0 = ins.Cnb;
% ins.qnb = qupdt(ins.qnb, ins.wnb*nts); % lower accuracy than next line
ins.qnb = qupdt2(ins.qnb, phim, ins.eth.wnin*nts);
[ins.qnb, ins.att, ins.Cnb] = attsyn(ins.qnb);
ins.avp = [ins.att; ins.vn; ins.pos];利用四元数进行姿态更新；
其中用到了三角函数和单位四元数关系：
5、误差图：