捷联惯导系统学习7.1(捷联惯导粗对准 )

初始对准：确定导航参数姿态、方位、速度、位置的初始值，方位的初始化是最难的。

单独的惯导难以长时间维持高精度定位导航，需要与其他方式综合。

捷联惯导粗对准

初始对准具体就是确定导航参考坐标系的一个过程，寻找导航坐标系（一般为东北天坐标系）与载体坐标系之间的对应关系。
双矢量定姿
方法一

已知两个不共线的矢量V1,V2V_1,V_2V1,V2,在坐标r（导航坐标系）系投影为V1r,V2rV^r_1,V_2^rV1r,V2r,b系（载体坐标系）下的投影为V1b,V2bV_1^b,V^b_2V1b,V2b,通过两个投影求解b系与r系之间关系，称为双矢量定姿。
求解：即两个坐标系之间的转换关系为CbrC^r_bCbr
存在：
V1r=CbrV1bV2r=CbrV2bV_1^r=C_b^rV^b_1 \\V_2^r=C_b^rV_2^bV1r=CbrV1bV2r=CbrV2b
构造：
V1r×V2r=(CbrV1b)×(CbrV2b)=Cbr(V1b×V2b)V^r_1×V^r_2=(C_b^rV^b_1)×(C_b^rV_2^b)=C_b^r(V_1^b×V_2^b)V1r×V2r=(CbrV1b)×(CbrV2b)=Cbr(V1b×V2b)
得到矩阵方程：
[V1rV2rV1r×V2r]=Cbr[V1bV2bV1b×V2b]\left[\begin{matrix} V_1^r&V^r_2&V_1^r×V_2^r\\ \end{matrix}\right]=C_b^r\left[\begin{matrix} V_1^b&V^b_2&V_1^b×V_2^b\\ \end{matrix}\right][V1rV2rV1r×V2r]=Cbr[V1bV2bV1b×V2b]
得到CbrC_b^rCbr
Cbr=[V1rV2rV1r×V2r][V1bV2bV1b×V2b]−1C^r_b=\left[\begin{matrix} V_1^r&V^r_2&V_1^r×V_2^r\\ \end{matrix}\right]\left[\begin{matrix} V_1^b&V^b_2&V_1^b×V_2^b\\ \end{matrix}\right]^{-1}Cbr=[V1rV2rV1r×V2r][V1bV2bV1b×V2b]−1
因为CbrC_b^rCbr是单位正交矩阵，Cbr=(Cbr)−TC^r_b=(C_b^r)^{-T}Cbr=(Cbr)−T再得到：
Cbr=[(V1r)T(V2r)T(V1r×V2r)T]−1[V1rV2rV1r×V2r]C_b^r=\left[\begin{matrix} (V_1^r)^T\\(V^r_2)^T\\(V_1^r×V_2^r)^T\\ \end{matrix}\right]^{-1}\left[\begin{matrix}V_1^r\\V^r_2\\V_1^r×V_2^r\\ \end{matrix}\right]Cbr=⎣⎡(V1r)T(V2r)T(V1r×V2r)T⎦⎤−1⎣⎡V1rV2rV1r×V2r⎦⎤

方法二
因为矢量误差往往包含模值误差和方向误差，方法一求取的模值不能严格满足单位正交化，方法2在此基础上，进行了修改。

已知测量得到了两个矢量V~1b,V~2b\tilde V_1^b,\tilde V_2^bV~1b,V~2b,两个V~1r,V~2r\tilde V_1^r,\tilde V_2^rV~1r,V~2r
V~1b,V~2b\tilde V_1^b,\tilde V_2^bV~1b,V~2b求解三个单位正交矢量,(必定两两正交)v1=V~1b∣V~1b∣;v2=V~1b×V~2b∣V~1b×V~2b∣;v3=V~1b×V~2b×V~1b∣V~1b×V~2b×V~1b∣v_1=\frac{\tilde V_1^b}{|\tilde V_1^b|};v_2=\frac{\tilde V_1^b×\tilde V_2^b}{|\tilde V_1^b×\tilde V_2^b|};v_3=\frac{\tilde V_1^b×\tilde V_2^b×\tilde V_1^b}{|\tilde V_1^b×\tilde V_2^b×\tilde V_1^b|}v1=∣V~1b∣V~1b;v2=∣V~1b×V~2b∣V~1b×V~2b;v3=∣V~1b×V~2b×V~1b∣V~1b×V~2b×V~1b
V~1r,V~2r\tilde V_1^r,\tilde V_2^rV~1r,V~2r构造三个正交单位矢量：
u1=V~1r∣V~1r∣;u2=V~1r×V~2r∣V~1r×V~2r∣;u3=V~1b×V~2r×V~1r∣V~1r×V~2r×V~1r∣u_1=\frac{\tilde V_1^r}{|\tilde V_1^r|};u_2=\frac{\tilde V_1^r×\tilde V_2^r}{|\tilde V_1^r×\tilde V_2^r|};u_3=\frac{\tilde V_1^b×\tilde V_2^r×\tilde V_1^r}{|\tilde V_1^r×\tilde V_2^r×\tilde V_1^r|}u1=∣V~1r∣V~1r;u2=∣V~1r×V~2r∣V~1r×V~2r;u3=∣V~1r×V~2r×V~1r∣V~1b×V~2r×V~1r
构造单位阵求解CbrC^r_bCbr
Cbr=[u1u2u3]−1[v1v2v3]=[u1u2u3][v1v2v3]C^r_b=\left[\begin{matrix}u_1\\u_2\\u_3\\ \end{matrix}\right]^{-1}\left[\begin{matrix}v_1\\v_2\\v_3\\ \end{matrix}\right]=\left[\begin{matrix}u_1&u_2&u_3\\ \end{matrix}\right]\left[\begin{matrix}v_1\\v_2\\v_3\\ \end{matrix}\right]Cbr=⎣⎡u1u2u3⎦⎤−1⎣⎡v1v2v3⎦⎤=[u1u2u3]⎣⎡v1v2v3⎦⎤

多矢量定姿
当有m(m>2)m(m>2)m(m>2)个不共面的矢量,在bbb系和rrr系对这些矢量进行测量，由于误差存在，可以近似满足如下关系：
V~ir≈CbrV~ib(i=1,2,...,m)\tilde V^r_i \approx C^r_b \tilde V^b_i(i=1,2,...,m)V~ir≈CbrV~ib(i=1,2,...,m)

这里求解变换矩阵变为了最优问题，指标函数为：
J∗(Cbr)=12∑i=1mwi∣V~ir−CbrV~ib∣2=min∑i=1mwi=1,加权平均时wi=1或wi=1/mJ^*(C^r_b)=\frac{1}{2}\sum^m_{i=1}w_i|\tilde V^r_i-C^r_b\tilde V^b_i|^2=min\\ \sum^m_{i=1}w_i=1,加权平均时w_i=1或w_i=1/mJ∗(Cbr)=21i=1∑mwi∣V~ir−CbrV~ib∣2=mini=1∑mwi=1,加权平均时wi=1或wi=1/m
求∣V~ir−CbrV~ib∣2|\tilde V^r_i-C^r_b\tilde V^b_i|^2∣V~ir−CbrV~ib∣2:
∣V~ir−CbrV~ib∣2=(V~ir−CbrV~ib)T(V~ir−CbrV~ib)=[(V~ir)T−(V~ib)T(Cbr)T](V~ir−CbrV~ib)=∣V~ir∣2+∣V~ib∣2−2(V~ir)CbrV~ib|\tilde V^r_i-C^r_b\tilde V^b_i|^2=(\tilde V^r_i-C^r_b\tilde V^b_i)^T(\tilde V^r_i-C^r_b\tilde V^b_i) \\ =[(\tilde V^r_i)^T-(\tilde V^b_i)^T(C^r_b)^T](\tilde V^r_i-C^r_b\tilde V^b_i) \\ =|\tilde V^r_i|^2+|\tilde V^b_i|^2-2(\tilde V^r_i)C^r_b\tilde V^b_i∣V~ir−CbrV~ib∣2=(V~ir−CbrV~ib)T(V~ir−CbrV~ib)=[(V~ir)T−(V~ib)T(Cbr)T](V~ir−CbrV~ib)=∣V~ir∣2+∣V~ib∣2−2(V~ir)CbrV~ib
带入得到：
J∗(Cbr)=12∑i=1mwi∣V~ir−CbrV~ib∣2=12∑i=1mwi(∣V~ir∣2+∣V~ib∣2)−∑i=1mwi(V~ir)TCbrV~ibJ^*(C^r_b)=\frac{1}{2}\sum^m_{i=1}w_i|\tilde V^r_i-C^r_b\tilde V^b_i|^2=\frac{1}{2}\sum^m_{i=1}w_i(|\tilde V^r_i|^2+|\tilde V_i^b|^2)-\sum^m_{i=1}w_i(\tilde V^r_i)^TC^r_b\tilde V^b_iJ∗(Cbr)=21i=1∑mwi∣V~ir−CbrV~ib∣2=21i=1∑mwi(∣V~ir∣2+∣V~ib∣2)−i=1∑mwi(V~ir)TCbrV~ib
想要J∗(Cbr)J^*(C^r_b)J∗(Cbr)取最小，即∑i=1mwi(V~ir)TCbrV~ib\sum^m_{i=1}w_i(\tilde V^r_i)^TC^r_b\tilde V^b_i∑i=1mwi(V~ir)TCbrV~ib取最大值
令J(Cbr)={∑i=1mwi(V~ir)TCbrV~ib}max令J(C^r_b)=\{\sum^m_{i=1}w_i(\tilde V^r_i)^TC^r_b\tilde V^b_i\}_{max}令J(Cbr)={i=1∑mwi(V~ir)TCbrV~ib}max
仅以化简：
J(Cbr)={∑i=1mwi(V~ir)TCbrV~ib}max=tr[[w1(V~1r)Tw2(V~2r)T...wm(V~mr)T]Cbr][V~1bV~2b...V~mb]]=tr(Cbr∑i=1mwiC~ib(C~ib)T)=tr(CbrAT)J(C^r_b)=\{\sum^m_{i=1}w_i(\tilde V^r_i)^TC^r_b\tilde V^b_i\}_{max}\\ =tr[\left[\begin{matrix}w_1(\tilde V_1^r)^T\\w_2(\tilde V_2^r)^T\\...\\w_m(\tilde V_m^r)^T \end{matrix}\right]C^r_b]\left[\begin{matrix}\tilde V_1^b&\tilde V_2^b&...&\tilde V_m^b \end{matrix}\right]]\\ =tr(C^r_b\sum^m_{i=1}w_i\tilde C_i^b(\tilde C_i^b)^T)\\ =tr(C^r_b A^T)J(Cbr)={i=1∑mwi(V~ir)TCbrV~ib}max=tr[⎣⎢⎢⎡w1(V~1r)Tw2(V~2r)T...wm(V~mr)T⎦⎥⎥⎤Cbr][V~1bV~2b...V~mb]]=tr(Cbri=1∑mwiC~ib(C~ib)T)=tr(CbrAT)
得到AT=∑i=1mwiC~ib(C~ib)TA^T=\sum^m_{i=1}w_i\tilde C_i^b(\tilde C_i^b)^TAT=∑i=1mwiC~ib(C~ib)T
对A进行奇异值分解即：A=UDVTA=UDV^TA=UDVT
得到 C^br=UVT\hat C^r_b=UV^TC^br=UVT

粗对准实现

初始对准时运载体一般实在精致条件下进行的，重力矢量和地球自转角速度矢量wiew_{ie}wie是已知的，分别如下:
gn=[00−g],wien=[0wiecosLwiesinL]=[0wNwU]g^n=\left[\begin{matrix} 0\\0\\-g \end{matrix}\right],w^n_{ie}=\left[\begin{matrix} 0\\w_{ie}cosL\\w_{ie}sinL \end{matrix}\right]=\left[\begin{matrix} 0\\w_N\\w_U \end{matrix}\right]gn=⎣⎡00−g⎦⎤,wien=⎣⎡0wiecosLwiesinL⎦⎤=⎣⎡0wNwU⎦⎤

地球角速度测量关系和比力方程
wibb:b系（载体坐标系）与i系（惯性坐标系）之间的旋转，在b系下的表示w^b_{ib}:b系（载体坐标系）与i系（惯性坐标系）之间的旋转，在b系下的表示wibb:b系（载体坐标系）与i系（惯性坐标系）之间的旋转，在b系下的表示
wibn：n系（惯导坐标系）与i系（惯性坐标系）之间的旋转（自转）在b系下的表示w^n_{ib}：n系（惯导坐标系）与i系（惯性坐标系）之间的旋转（自转）在b系下的表示wibn：n系（惯导坐标系）与i系（惯性坐标系）之间的旋转（自转）在b系下的表示
wenn:n系（惯导坐标系）与e系（地球坐标系）之间的夹角在n系下的表示w^n_{en}:n系（惯导坐标系）与e系（地球坐标系）之间的夹角在n系下的表示wenn:n系（惯导坐标系）与e系（地球坐标系）之间的夹角在n系下的表示
wnbn:基静坐晃动干扰角速度w^n_{nb}:基静坐晃动干扰角速度wnbn:基静坐晃动干扰角速度
v˙n:寂静做晃动角速度\dot v^n:寂静做晃动角速度v˙n:寂静做晃动角速度
fsfb:比力加速度测得数据f^b_{sf}:比力加速度测得数据fsfb:比力加速度测得数据
Cbnwibb=wibn=wien+wenn+wnbnv˙n=Cbnfsfb−(2wien+wenn)×vn+gnC^n_bw^b_{ib}=w^n_{ib}=w_{ie}^n+w^n_{en}+w^n_{nb}\\ \dot v^n=C^n_bf^b_{sf}-(2w^n_{ie}+w^n_{en})×v^n+g^nCbnwibb=wibn=wien+wenn+wnbnv˙n=Cbnfsfb−(2wien+wenn)×vn+gn
静基座状态下(2wien+wenn)×vn(2w^n_{ie}+w^n_{en})×v^n(2wien+wenn)×vn和wennw^n_{en}wenn可以忽略不计，再加入陀螺仪量测误差δwibb\delta w^b_{ib}δwibb和加速度计测量误差δfsfb\delta f^b_{sf}δfsfb,得到：
Cbn(w~ibb−δwibb)−wnbn=wienCbn(f~sfb−δfsfb)−v˙n=−gnC^n_b(\tilde w^b_{ib}-\delta w^b_{ib})-w^n_{nb}=w^n_{ie} \\ C^n_b(\tilde f^b_{sf}-\delta f^b_{sf})-\dot v^n=-g^n Cbn(w~ibb−δwibb)−wnbn=wienCbn(f~sfb−δfsfb)−v˙n=−gn
可以简写为：
ξ‾n=Cbbw~ibb+wnbn\overline \xi^n=C^b_b\tilde w^b_{ib}+w^n_{nb}ξn=Cbbw~ibb+wnbn
▽‾n=Cbnδfsfb+v˙n\overline\bigtriangledown^n=C^n_b\delta f^b_{sf}+\dot v^n▽n=Cbnδfsfb+v˙n
Cbnw~ibb−ξ‾n=wienCbnf~sfb−▽‾n=−gnC^n_b\tilde w^b_{ib}-\overline \xi^n=w^n_{ie}\\ C^n_b \tilde f^b_{sf}-\overline\bigtriangledown^n=-g^nCbnw~ibb−ξn=wienCbnf~sfb−▽n=−gn
当测量误差远小于有用信号时，即∣ξ‾n∣<110∣wien∣=wie10|\overline \xi^n|<\frac{1}{10}|w^n_{ie}|=\frac{w_{ie}}{10}∣ξn∣<101∣wien∣=10wie且∣▽‾n∣<∣−gn∣100=g100|\overline\bigtriangledown^n|<\frac{|-g^n|}{100}=\frac{g}{100}∣▽n∣<100∣−gn∣=100g,上式可以近似估计为：
C~bnw~ibb=wienC~bnf~sfb=−gn\tilde C^n_b \tilde w^b_{ib}=w^n_{ie}\\ \tilde C^n_b \tilde f^b_{sf}=-g^nC~bnw~ibb=wienC~bnf~sfb=−gn
在一般情况下，线运动干扰相对误差小于角运动，参考双矢量标定，以(−gn)(-g^n)(−gn)作为主参考矢量，可以得到姿态估计阵为：
C^bn=[(−gn)∣(−gn)∣(−gn)×wien∣(−gn)×wien∣(−gn)×wien×(−gn)∣(−gn)×wien×(−gn)∣]×[(f~sfn)T∣f~sfn∣f~sfn×w~ibb∣f~sfn×w~ibb∣(f~sfn×w~ibb×f~sfn)∣f~sfn×w~ibb×f~sfn∣]\hat C_b^n=\left[\begin{matrix} \frac{(-g^n)}{|(-g^n)|}&\frac{(-g^n)×w^n_{ie}}{|(-g^n)×w^n_{ie}|}&\frac{(-g^n)×w^n_{ie}×(-g^n)}{|(-g^n)×w^n_{ie}×(-g^n)|}\\ \end{matrix}\right]×\left[\begin{matrix} \frac{(\tilde f^n_{sf})^T}{|\tilde f^n_{sf}|}\\\frac{\tilde f^n_{sf}×\tilde w^b_{ib}}{|\tilde f^n_{sf}×\tilde w^b_{ib}|}\\ \frac{(\tilde f^n_{sf}×\tilde w^b_{ib}×\tilde f^n_{sf})}{|\tilde f^n_{sf}×\tilde w^b_{ib}×\tilde f^n_{sf}|}\\ \end{matrix}\right]C^bn=[∣(−gn)∣(−gn)∣(−gn)×wien∣(−gn)×wien∣(−gn)×wien×(−gn)∣(−gn)×wien×(−gn)]×⎣⎢⎢⎢⎢⎡∣f~sfn∣(f~sfn)T∣f~sfn×w~ibb∣f~sfn×w~ibb∣f~sfn×w~ibb×f~sfn∣(f~sfn×w~ibb×f~sfn)⎦⎥⎥⎥⎥⎤
将gn,wieng^n,w^n_{ie}gn,wien带入，求得C^bn\hat C^n_bC^bn
C^bn=[0−10001100][(f~sfn)T∣f~sfn∣f~sfn×w~ibb∣f~sfn×w~ibb∣(f~sfn×w~ibb×f~sfn)∣f~sfn×w~ibb×f~sfn∣]=[−(f~sfn×w~ibb)T∣f~sfn×w~ibb∣(f~sfn×w~ibb×f~sfn)T∣f~sfn×w~ibb×f~sfn∣(f~sfn)T∣f~sfn∣]\hat C^n_b=\left[\begin{matrix} 0&-1&0\\0&0&1\\1&0&0 \end{matrix}\right]\left[\begin{matrix} \frac{(\tilde f^n_{sf})^T}{|\tilde f^n_{sf}|}\\\frac{\tilde f^n_{sf}×\tilde w^b_{ib}}{|\tilde f^n_{sf}×\tilde w^b_{ib}|}\\ \frac{(\tilde f^n_{sf}×\tilde w^b_{ib}×\tilde f^n_{sf})}{|\tilde f^n_{sf}×\tilde w^b_{ib}×\tilde f^n_{sf}|}\\ \end{matrix}\right] =\left[\begin{matrix} -\frac{(\tilde f^n_{sf}×\tilde w^b_{ib})^T}{|\tilde f^n_{sf}×\tilde w^b_{ib}|}\\ \frac{(\tilde f^n_{sf}×\tilde w^b_{ib}×\tilde f^n_{sf})^T}{|\tilde f^n_{sf}×\tilde w^b_{ib}×\tilde f^n_{sf}|}\\ \frac{(\tilde f^n_{sf})^T}{|\tilde f^n_{sf}|}\\ \end{matrix}\right] C^bn=⎣⎡001−100010⎦⎤⎣⎢⎢⎢⎢⎡∣f~sfn∣(f~sfn)T∣f~sfn×w~ibb∣f~sfn×w~ibb∣f~sfn×w~ibb×f~sfn∣(f~sfn×w~ibb×f~sfn)⎦⎥⎥⎥⎥⎤=⎣⎢⎢⎢⎢⎡−∣f~sfn×w~ibb∣(f~sfn×w~ibb)T∣f~sfn×w~ibb×f~sfn∣(f~sfn×w~ibb×f~sfn)T∣f~sfn∣(f~sfn)T⎦⎥⎥⎥⎥⎤

7.实际中：∠(w~ibb,f~sfb)≈π2−L\angle (\tilde w^b_{ib},\tilde f^b_{sf})\approx \frac{\pi}{2}-L∠(w~ibb,f~sfb)≈2π−L
假设角增量为在[0,T][0,T][0,T]时间内Δθ(T)\Delta \theta(T)Δθ(T)速度增量为ΔV(T)\Delta V(T)ΔV(T)，当[0,T][0,T][0,T]时间内低频晃动小于Wie/10W_{ie}/10Wie/10且速度变化小于gT/100gT/100gT/100时能够得到一定经度的结果
令w~ibb=w‾ibb=Δθ(T)T,f~sfb=f‾sfb=ΔV(T)T\tilde w^b_{ib}=\overline w^b_{ib}=\frac{\Delta \theta(T)}{T},\tilde f^b_{sf}=\overline f^b_{sf}=\frac{\Delta V(T)}{T}w~ibb=wibb=TΔθ(T),f~sfb=fsfb=TΔV(T)

粗对准方法的误差分析
C^bn=[0−10001100][(f~sfn)T∣f~sfn∣f~sfn×w~ibb∣f~sfn×w~ibb∣(f~sfn×w~ibb×f~sfn)∣f~sfn×w~ibb×f~sfn∣]=[−(f~sfn×w~ibb)T∣f~sfn×w~ibb∣(f~sfn×w~ibb×f~sfn)T∣f~sfn×w~ibb×f~sfn∣(f~sfn)T∣f~sfn∣]\hat C^n_b=\left[\begin{matrix} 0&-1&0\\0&0&1\\1&0&0 \end{matrix}\right]\left[\begin{matrix} \frac{(\tilde f^n_{sf})^T}{|\tilde f^n_{sf}|}\\\frac{\tilde f^n_{sf}×\tilde w^b_{ib}}{|\tilde f^n_{sf}×\tilde w^b_{ib}|}\\ \frac{(\tilde f^n_{sf}×\tilde w^b_{ib}×\tilde f^n_{sf})}{|\tilde f^n_{sf}×\tilde w^b_{ib}×\tilde f^n_{sf}|}\\ \end{matrix}\right] =\left[\begin{matrix} -\frac{(\tilde f^n_{sf}×\tilde w^b_{ib})^T}{|\tilde f^n_{sf}×\tilde w^b_{ib}|}\\ \frac{(\tilde f^n_{sf}×\tilde w^b_{ib}×\tilde f^n_{sf})^T}{|\tilde f^n_{sf}×\tilde w^b_{ib}×\tilde f^n_{sf}|}\\ \frac{(\tilde f^n_{sf})^T}{|\tilde f^n_{sf}|}\\ \end{matrix}\right] C^bn=⎣⎡001−100010⎦⎤⎣⎢⎢⎢⎢⎡∣f~sfn∣(f~sfn)T∣f~sfn×w~ibb∣f~sfn×w~ibb∣f~sfn×w~ibb×f~sfn∣(f~sfn×w~ibb×f~sfn)⎦⎥⎥⎥⎥⎤=⎣⎢⎢⎢⎢⎡−∣f~sfn×w~ibb∣(f~sfn×w~ibb)T∣f~sfn×w~ibb×f~sfn∣(f~sfn×w~ibb×f~sfn)T∣f~sfn∣(f~sfn)T⎦⎥⎥⎥⎥⎤
上述分母可以近似为：
∣f~sfn)×w~ibb∣≈∣(−gn)×wien∣=gwiecosL=gwn|\tilde f^n_{sf})×\tilde w^b_{ib}| \approx |(-g^n)×w^n_{ie}|=gw_{ie}cosL=gw_n∣f~sfn)×w~ibb∣≈∣(−gn)×wien∣=gwiecosL=gwn
∣f~sfn×w~ibb×f~sfn∣≈∣(−gn)×wien×(−gn)∣=g2wN|\tilde f^n_{sf}×\tilde w^b_{ib}×\tilde f^n_{sf}| \approx|(-g^n)×w^n_{ie}×(-g^n)|=g^2w_N∣f~sfn×w~ibb×f~sfn∣≈∣(−gn)×wien×(−gn)∣=g2wN
∣f~sfn∣≈∣(−gn)∣=g|\tilde f^n_{sf}|\approx |(-g^n)|=g∣f~sfn∣≈∣(−gn)∣=g
C^bn\hat C^n_bC^bn忽略误差的高阶小量：
C^bn≈[−[(fsfb+δfsfb)×(wibb+δwibb)]/(gwN)−[(fsfb+δfsfb)×(wibb+δwibb)×(fsfb+δfsfb)]T/(g2wN)(fsfb+δfsfb)T/g]≈[I+[∗−δwib,En/wN+δfsf,EntanL/g∗∗∗∗δfsf,En/gδfsf,Nn/g∗]]Cbn\hat C^n_b \approx \left[\begin{matrix} -[(f^b_{sf}+\delta f^b_{sf})×(w^b_{ib}+\delta w^b_{ib})]/(gw_N)\\ -[(f^b_{sf}+\delta f^b_{sf})×(w^b_{ib}+\delta w^b_{ib})×(f^b_{sf}+\delta f^b_{sf})]^T/(g^2w_N)\\ (f^b_{sf}+\delta f^b_{sf})^T/g \end{matrix}\right] \\ \approx [I+\left[\begin{matrix} *&-\delta w^n_{ib,E}/w_N+\delta f^n_{sf,E}tanL/g&*\\*&*&*\\\delta f^n_{sf,E}/g&\delta f^n_{sf,N}/g&* \end{matrix}\right]]C^n_bC^bn≈⎣⎡−[(fsfb+δfsfb)×(wibb+δwibb)]/(gwN)−[(fsfb+δfsfb)×(wibb+δwibb)×(fsfb+δfsfb)]T/(g2wN)(fsfb+δfsfb)T/g⎦⎤≈[I+⎣⎡∗∗δfsf,En/g−δwib,En/wN+δfsf,EntanL/g∗δfsf,Nn/g∗∗∗⎦⎤]Cbn
又因为C^bn=(I−ϕ×)Cbn(ϕ为失准角)\hat C^n_b=(I-\phi×)C^n_b(\phi为失准角)C^bn=(I−ϕ×)Cbn(ϕ为失准角)
求得失准角为：
ϕ=[−δfsf,Nn/gδfsf,En/g−δwib,En/wN+δfsf,EntanL/g]≈[−δfsf,Nn/gδfsf,En/g−δwib,En/wN]\phi=\left[\begin{matrix} -\delta f^n_{sf,N}/g\\\delta f^n_{sf,E}/g\\-\delta w^n_{ib,E}/w_N+\delta f^n_{sf,E}tanL/g \end{matrix}\right] \approx \left[\begin{matrix} -\delta f^n_{sf,N}/g\\\delta f^n_{sf,E}/g\\-\delta w^n_{ib,E}/w_N \end{matrix}\right] ϕ=⎣⎡−δfsf,Nn/gδfsf,En/g−δwib,En/wN+δfsf,EntanL/g⎦⎤≈⎣⎡−δfsf,Nn/gδfsf,En/g−δwib,En/wN⎦⎤
可以观察到失准角的主要无影响。

间接粗对准方法

与直接对准方法相比，短时间内抗扰能力弱，长时间（分重量级）间接粗对准方法更加准确

设：
初始时刻载体惯性系b0b_0b0,与对准时刻载体惯性系重合
初始时刻导航惯性系n0n_0n0，与对准时刻导航惯性系重合
常值矩阵Cb0n0C^{n_0}_{b_0}Cb0n0为b0b_0b0系与n0n_0n0系的方位关系
重力矢量，在n0n_0n0系的投影：
gn=[00−g]Tg^n=\left[\begin{matrix}0&0&-g\end{matrix}\right]^Tgn=[00−g]T
gn0=Cnn0gnC˙nn0=Cnn0(wn0nn×)=Cnn0wien×g^{n_0}=C_n^{n_0}g^n \\ \dot C^{n_0}_{n}=C^{n_0}_{n}(w^n_{n_0n}\times)=C_n^{n_0}{w^n_{ie}\times}gn0=Cnn0gnC˙nn0=Cnn0(wn0nn×)=Cnn0wien×
解得Cnn0=eiwien×=I+sinwietwiet(twien×)+1−coswiet(wiet)2(twien×)2=[coswiet−sinwietsinLsinwietcosLsinwietsinL1−(1−coswiet)sin2L(1−coswiet)sinLcosL−sinwietcosL(1−coswiet)sinLcosL1−(1−coswiet)cos2L]C^{n_0}_n=e^{i w_{ie}^n\times}=I+\frac{sin{w_{ie}}t}{w_{ie}t}(tw^n_{ie}\times)+\frac{1-cosw_{ie}t}{(w_{ie}t)^2}(tw^n_{ie}\times)^2\\=\left[\begin{matrix} cosw_{ie}t&-sinw_{ie}tsinL&sinw_{ie}tcosL\\ sinw_{ie}tsinL&1-(1-cosw_{ie}t)sin^2L&(1-cosw_{ie}t)sinLcosL\\ -sinw_{ie}tcosL&(1-cosw_{ie}t)sinLcosL&1-(1-cosw_{ie}t)cos^2L\\ \end{matrix}\right]Cnn0=eiwien×=I+wietsinwiet(twien×)+(wiet)21−coswiet(twien×)2=⎣⎡coswietsinwietsinL−sinwietcosL−sinwietsinL1−(1−coswiet)sin2L(1−coswiet)sinLcosLsinwietcosL(1−coswiet)sinLcosL1−(1−coswiet)cos2L⎦⎤
得到gn0=−g[sinwiet(1−coswiet))sinLcosL1−(1−coswiet))cos2L]g^{n_0}=-g\left[\begin{matrix} sinw_{ie}t\\ (1-cosw_{ie}t))sinLcosL\\ 1-(1-cosw_{ie}t))cos^2L\\ \end{matrix}\right]gn0=−g⎣⎡sinwiet(1−coswiet))sinLcosL1−(1−coswiet))cos2L⎦⎤
加速度计的比例输出在b0b_0b0系投影为：
wibb:为陀螺仪测量值Cbb0(0)=I:初始值为I，其他时刻值使用姿态更新算法（二子样法或者其他）w^b_{ib}:为陀螺仪测量值\\ C_b^{b_0}(0)=I:初始值为I，其他时刻值使用姿态更新算法（二子样法或者其他）wibb:为陀螺仪测量值Cbb0(0)=I:初始值为I，其他时刻值使用姿态更新算法（二子样法或者其他）
fsfb0=Cbb0fsfbC˙bb0=Cbb0(wb0bb×)=Cbb0(wibb×)f^{b_0}_{sf}=C_b^{b_0} f^b_{sf}\\ \dot C_b^{b_0}=C_b^{b_0}(w^b_{b_0b}\times)=C_b^{b_0}(w^b_{ib}\times)fsfb0=Cbb0fsfbC˙bb0=Cbb0(wb0bb×)=Cbb0(wibb×)
得到加速度与加速度计比力测量之间关系：
▽‾b0:表示在b0系的加速度计测量误差及线性干扰\overline \bigtriangledown^{b_0}:表示在b_0系的加速度计测量误差及线性干扰▽b0:表示在b0系的加速度计测量误差及线性干扰
Cb0n0(Cbb0f~sfb−▽‾b0)=−gn0▽‾b0=Cbb0δfsfb+v˙b0C_{b_0}^{n_0}(C_b^{b_0}\tilde f^b_{sf}-\overline \bigtriangledown^{b_0})=-g^{n_0}\\ \overline \bigtriangledown^{b_0}=C_b^{b_0}\delta f^b_{sf}+\dot v^{b_0}Cb0n0(Cbb0f~sfb−▽b0)=−gn0▽b0=Cbb0δfsfb+v˙b0
在初始对准过程中进行定积分得到：
{F~ib0=∫0tiCbb0f~sfbdtGin0=−∫0tign0dt\begin{cases}\tilde F_i^{b_0}=\int_0^{t_i}C_{b}^{b_0}\tilde f_{sf}^bdt\\ G_i^{n_0}=-\int_0^{t_i}g^{n0}dt\\ \end{cases}{F~ib0=∫0tiCbb0f~sfbdtGin0=−∫0tign0dt
得到Gin0=Cb0b0F~ib0−∫0tiCb0n0▽‾b0dtG_i^{n_0}=C_{b0}^{b_0}\tilde F_i^{b_0}-\int_0^{t_i}C_{b_0}^{n_0}\overline \bigtriangledown^{b_0}dtGin0=Cb0b0F~ib0−∫0tiCb0n0▽b0dt
使用双重矢量法对准：
在t2t_2t2时间内结束对准：t2=2t1t_2=2t_1t2=2t1最后得到:
C^b0n0=[G1n0∣G1n0∣G1n0×G2n0∣G1n0×G2n0∣G1n0×G2n0×G1n0∣G1n0×]×[(F~1b0)T∣F~1b0∣(F~1b0×F~2b0)T∣F~1b0×F~2b0∣(F~1b0×F~2b0×F~1b0)T∣F~1b0×F~2b0×F~1b0T∣]\hat C^{n_0}_{b_0}=\left[\begin{matrix} \frac{G_1^{n_0}}{|G_1^{n_0}|}&\frac{G_1^{n_0}\times G_2^{n_0} }{|G_1^{n_0}\times G_2^{n_0} |}&\frac{G_1^{n_0}\times G_2^{n_0}\times G_1^{n_0} }{|G_1^{n_0}\times } \end{matrix}\right]\times\\ \left[\begin{matrix} \frac{(\tilde F_1^{b_0})^T}{|\tilde F_1^{b_0}|}\\ \frac{(\tilde F_1^{b_0} \times \tilde F_2^{b_0})^T}{|\tilde F_1^{b_0} \times \tilde F_2^{b_0}|}\\ \frac{(\tilde F_1^{b_0} \times \tilde F_2^{b_0}\times \tilde F_1^{b_0})^T}{|\tilde F_1^{b_0} \times \tilde F_2^{b_0}\times \tilde F_1^{b_0}T|} \end{matrix}\right]C^b0n0=[∣G1n0∣G1n0∣G1n0×G2n0∣G1n0×G2n0∣G1n0×G1n0×G2n0×G1n0]×⎣⎢⎢⎢⎢⎡∣F~1b0∣(F~1b0)T∣F~1b0×F~2b0∣(F~1b0×F~2b0)T∣F~1b0×F~2b0×F~1b0T∣(F~1b0×F~2b0×F~1b0)T⎦⎥⎥⎥⎥⎤
使用多重矢量法标定：
A=−∫0t2gn0(f~sfb0)TdtA=-\int_0^{t_2}g^{n_0}(\tilde f_{sf}^{b_0})^TdtA=−∫0t2gn0(f~sfb0)Tdt
再进行求解
得到最后对准时刻矩阵C^b0n0\hat C^{n_0}_{b_0}C^b0n0
C˙nn0=Cnn0(wn0nn×)=Cnn0wien×C˙bb0=Cbb0(wb0bb×)=Cbb0(wibb×)\dot C^{n_0}_{n}=C^{n_0}_{n}(w^n_{n_0n}\times)=C_n^{n_0}{w^n_{ie}\times}\\ \dot C_b^{b_0}=C_b^{b_0}(w^b_{b_0b}\times)=C_b^{b_0}(w^b_{ib}\times)C˙nn0=Cnn0(wn0nn×)=Cnn0wien×C˙bb0=Cbb0(wb0bb×)=Cbb0(wibb×)
C^b0n0=Cn0nC^b0n0Cbb0\hat C^{n_0}_{b_0}=C^n_{n0}\hat C_{b_0}^{n_0}C_b^{b_0}C^b0n0=Cn0nC^b0n0Cbb0