捷联惯导系统学习7.4(车载惯性/里程仪组合导航 )

航位推算算法（Dead Reckoning，DR）

利用位姿、航向、行驶里程信息推算载车相对于起始点的位置。
里程仪（odometer）:输出一般是载车在一小段内行驶的路程增量，为了方便分析，假设里程仪输出的是瞬时速度。轮式车辆往往分为转向轮和非转向轮，非转向轮始终与车体正前方一致，这里假设里程仪测量的是非转向轮的信号。
理想情况下：载车正常形式，前进取正，倒车取负，且车轮紧贴地面无打滑、滑行、弹跳。设车体坐标系为m系，oym:在和载车车轮相接处的地平面内,ozm：垂直与地平面内向上为正;0xm指向右方oy_m:在和载车车轮相接处的地平面内,oz_m：垂直与地平面内向上为正;0x_m指向右方oym:在和载车车轮相接处的地平面内,ozm：垂直与地平面内向上为正;0xm指向右方
里程仪的输出在车体坐标系m可以表示为：
vDm=[0vD0]v^m_D=\left[\begin{matrix}0&v_D&0 \end{matrix}\right]vDm=[0vD0]
捷联惯导(IMU)固定在车体上，假设IMU坐标系（b系）与车体坐标系（m系）重合，通过IMU中3轴陀螺仪可以实时计算载车的姿态矩阵，记为CbnC_b^nCbn,可以利用CbnC^n_bCbn和vDnv^n_DvDn求得cDnc^n_DcDn在导航坐标系下的里程仪速度输出，即：
vDn=CbnvDnv^n_D=C^n_bv^n_DvDn=CbnvDn

利用vDnv^n_DvDn求得航位
LD:航位推算的地理纬度λD:航位推算的经度h:航位推算的高度L_D:航位推算的地理纬度\\ \lambda_D:航位推算的经度\\ h:航位推算的高度LD:航位推算的地理纬度λD:航位推算的经度h:航位推算的高度

RMhD=RMh+hD:RMh航位推算的地理位置计算的子午圈RNhD=RNh+hD:RNh航位推算的地理位置计算的卯酉圈R_{MhD}=R_{Mh}+h_D:R_{Mh}航位推算的地理位置计算的子午圈\\ R_{NhD}=R_{Nh}+h_D:R_{Nh}航位推算的地理位置计算的卯酉圈RMhD=RMh+hD:RMh航位推算的地理位置计算的子午圈RNhD=RNh+hD:RNh航位推算的地理位置计算的卯酉圈
vDn=[vDEvNDvDU]Tv^n_D=\left[\begin{matrix} v_{DE}&v_{ND}&v_{DU}\\ \end{matrix}\right]^TvDn=[vDEvNDvDU]T
L˙D=vDNRMhDλ˙=vDEsec⁡LDRNhDh˙D=vDU\dot L_D=\frac{v_{DN}}{R_{MhD}}\\ \dot \lambda=\frac{v_{DE}\sec L_D}{R_{NhD}}\\ \dot h_D=v_{DU}L˙D=RMhDvDNλ˙=RNhDvDEsecLDh˙D=vDU
可将上式简写为：
PD=[LDλDhD]TP_D=\left[\begin{matrix} L_D&\lambda_D&h_D\\ \end{matrix}\right]^TPD=[LDλDhD]T
MpvD=[01/RMhD0secLD/RNhD00001]P˙D=MpvvDnM_{pvD}=\left[\begin{matrix} 0&1/R_{MhD}&0\\secL_D/R_{NhD}&0&0\\0&0&1 \end{matrix}\right]\\ \dot P_D=M_{pv}v^n_DMpvD=⎣⎡0secLD/RNhD01/RMhD00001⎦⎤P˙D=MpvvDn
令航位推算纬度代替捷联惯导解算纬度,里程仪计算速度代替捷联惯导解算速度即：L=LD,vn=vDnL=L_D,v^n=v^n_DL=LD,vn=vDn得到航位推算姿态更新算法方程如下：
wibb:陀螺仪输出角速度信息w^b_{ib}:陀螺仪输出角速度信息wibb:陀螺仪输出角速度信息
winn=wien+wenn:wien=[0wiecosLDwiesinLD]Twenn=[−vDNRMhDvDERNhDvDEtanLDRNhD]Tw^n_{in}=w^n_{ie}+w^n_{en}:\\ w^n_{ie}=\left[\begin{matrix}0&w_{ie}cosL_D&w_{ie}sinL_D\end{matrix}\right]^T\\ w^n_{en}=\left[\begin{matrix}-\frac{v_{DN}}{R_{MhD}}&\frac{v_{DE}}{R_{NhD}}&\frac{v_{DE}tanL_D}{R_{NhD}}\end{matrix}\right]^Twinn=wien+wenn:wien=[0wiecosLDwiesinLD]Twenn=[−RMhDvDNRNhDvDERNhDvDEtanLD]T
C˙bn=Cbn(wibb×)−(winn×)Cbnwinn=wien+wenn\dot C^n_b=C^n_b(w^b_{ib}×)-(w^n_{in}×)C^n_b\\ w_{in}^{n}=w^n_{ie}+w^n_{en}C˙bn=Cbn(wibb×)−(winn×)Cbnwinn=wien+wenn
得到航位推算算法：
{P˙D=MpvvDnC˙bn=Cbn(wibb×)−(winn×)Cbnwinn=wien+wenn\begin{cases} \dot P_D=M_{pv}v^n_D\\ \dot C^n_b=C^n_b(w^b_{ib}×)-(w^n_{in}×)C^n_b&w_{in}^{n}=w^n_{ie}+w^n_{en}\\ \end{cases}{P˙D=MpvvDnC˙bn=Cbn(wibb×)−(winn×)Cbnwinn=wien+wenn

数值更新算法：

如果里程仪在Tj=tj−tj−1T_j=t_j-t_{j-1}Tj=tj−tj−1内的路程增量为ΔSj\Delta S_jΔSj,当时间足够短时，可以认为载车在着小段时间内时沿直线行驶，可以得到路程增量在m系的投影为：
ΔSjn=[0ΔSj0]T\Delta S^n_j=\left[\begin{matrix}0&\Delta S_j&0\end{matrix}\right]^TΔSjn=[0ΔSj0]T
类似可以得到:
Cb(j−1)是tj−1n时刻的载车姿态矩阵C^n_{b(j-1)是t_{j-1}}时刻的载车姿态矩阵Cb(j−1)是tj−1n时刻的载车姿态矩阵
ΔSjm=Cb(j−1)nΔSjm\Delta S^m_j=C^n_{b(j-1)}\Delta S^m_jΔSjm=Cb(j−1)nΔSjm
将数据离散化得到航位推算位置更新算法如下：
ΔSjn=[ΔSE(j)ΔSN(j)ΔSU(j)]T,分别为东、北、天向位移分量\Delta S^n_j=\left[\begin{matrix} \Delta S_E(j)&\Delta S_N(j)&\Delta S_U(j) \end{matrix}\right]^T,分别为东、北、天向位移分量ΔSjn=[ΔSE(j)ΔSN(j)ΔSU(j)]T,分别为东、北、天向位移分量
LD(j)=LD(j−1)+TjvDN(j)RMhD(j−1)=LD(j−1)+ΔSN(j)RMhD(j−1)λD(j)=λD(j−1)+TjvDE(j)secLD(j−1)RNhD(j−1)=λD(j−1)+ΔSE(j)secLD(j−1)RNhD(j−1)hD(j)=HD(j−1)+TjvDU=hD(j−1)+ΔSU(j)L_{D(j)}=L_{D(j-1)}+\frac{T_jv_{DN(j)}}{R_{MhD(j-1)}}=L_{D(j-1)}+\frac{\Delta S_{N(j)}}{R_{MhD(j-1)}}\\ \lambda _{D(j)}=\lambda _{D(j-1)}+\frac{T_jv_{DE(j)}secL_{D(j-1)}}{R_{NhD(j-1)}}=\lambda _{D(j-1)}+\frac{\Delta S_{E(j)}secL_{D(j-1)}}{R_{NhD(j-1)}}\\ h_{D(j)}=H_{D(j-1)}+T_j v_{DU}=h_{D(j-1)}+\Delta S_{U(j)}LD(j)=LD(j−1)+RMhD(j−1)TjvDN(j)=LD(j−1)+RMhD(j−1)ΔSN(j)λD(j)=λD(j−1)+RNhD(j−1)TjvDE(j)secLD(j−1)=λD(j−1)+RNhD(j−1)ΔSE(j)secLD(j−1)hD(j)=HD(j−1)+TjvDU=hD(j−1)+ΔSU(j)
航位推算的子态阵更新算法如下：
ϕib(j)b:由陀螺仪输出计算的等效旋转矢量\phi^b_{ib(j)}:由陀螺仪输出计算的等效旋转矢量ϕib(j)b:由陀螺仪输出计算的等效旋转矢量
Cb(j)n=Cn(j−1)n(j)Cb(j−1)nCb(j)(b(j−1))Cb(j)(b(j−1))=MRV(ϕib(j)b),Cn(j−1)n(j)=MRVT(ϕin(j)n)C^n_{b(j)}=C^{n(j)}_{n(j-1)}C^n_{b(j-1)}C^{(b(j-1))}_{b(j)}\\ C^{(b(j-1))}_{b(j)}=M_{RV}(\phi^b_{ib(j)}),C^{n(j)}_{n(j-1)}=M^T_{RV}(\phi^n_{in(j)})Cb(j)n=Cn(j−1)n(j)Cb(j−1)nCb(j)(b(j−1))Cb(j)(b(j−1))=MRV(ϕib(j)b),Cn(j−1)n(j)=MRVT(ϕin(j)n)
ϕin(j)n=Tj(wie(j)n+wen(j)n)=Tj[0wiecosLD(j)wiesinLD(j)]+Tj[−vDN(j)/RMhD(j)vDE(j)/RNhD(j)vDE(j)tanLD(j)/RNhD(j)]=Tj[0wiecosLD(j)wiesinLD(j)]+[−ΔSDN(j)/RMhD(j)ΔSDE(j)/RNhD(j)ΔSDE(j)vDE(j)tanLD(j)/RNhD(j)]\phi^n_{in(j)}=T_j(w^n_{ie(j)}+w^n_{en(j)})=T_j\left[\begin{matrix} 0\\ w_{ie}cosL_{D(j)}\\ w_{ie}sinL_{D(j)}\\ \end{matrix}\right]+T_j\left[\begin{matrix} -v_{DN(j)}/R_{MhD(j)}\\ v_{DE(j)}/R_{NhD(j)}\\ v_{DE(j)}tanL_{D(j)}/R_{NhD(j)}\\ \end{matrix}\right]\\ =T_j\left[\begin{matrix} 0\\ w_{ie}cosL_{D(j)}\\ w_{ie}sinL_{D(j)}\\ \end{matrix}\right]+\left[\begin{matrix} -\Delta S_{DN(j)}/R_{MhD(j)}\\ \Delta S_{DE(j)}/R_{NhD(j)}\\ \Delta S_{DE(j)}v_{DE(j)}tanL_{D(j)}/R_{NhD(j)}\\ \end{matrix}\right]ϕin(j)n=Tj(wie(j)n+wen(j)n)=Tj⎣⎡0wiecosLD(j)wiesinLD(j)⎦⎤+Tj⎣⎡−vDN(j)/RMhD(j)vDE(j)/RNhD(j)vDE(j)tanLD(j)/RNhD(j)⎦⎤=Tj⎣⎡0wiecosLD(j)wiesinLD(j)⎦⎤+⎣⎡−ΔSDN(j)/RMhD(j)ΔSDE(j)/RNhD(j)ΔSDE(j)vDE(j)tanLD(j)/RNhD(j)⎦⎤

航位推算算法误差分析

惯导坐标系(b系)与车体坐标系(m系)不平行存在误差，俯仰偏角αθ\alpha _\thetaαθ、方位偏角αϕ\alpha_{\phi}αϕ、滚动偏角αr\alpha_rαr,令偏差角矢量α=[αθαrαϕ]T\alpha=\left[\begin{matrix}\alpha_\theta&\alpha_r&\alpha_\phi\end{matrix}\right]^Tα=[αθαrαϕ]T
即b系与m系的变换矩阵为：
cbm=I+(α×)=[1−αϕαrαϕ1−αθ−αrαθ1]c^m_b=I+(\alpha×)=\left[\begin{matrix} 1&-\alpha_\phi&\alpha_r\\ \alpha_\phi&1&-\alpha_\theta\\ -\alpha_r&\alpha_\theta&1\\ \end{matrix}\right]cbm=I+(α×)=⎣⎡1αϕ−αr−αϕ1αθαr−αθ1⎦⎤
还存在刻度系数误差δKD\delta K_DδKD，得到实际速度大小v~D\tilde v_Dv~D与理论速度大小vDv_DvD存在如下关系:
v~D=(1+δKD)vD\tilde v_D=(1+\delta K_D)v_Dv~D=(1+δKD)vD
向量形式：
v~Dm=(1+δKD)vDm\tilde v^m_D=(1+\delta K_D)v^m_Dv~Dm=(1+δKD)vDm
在导航坐标系下的实际速度输出为：
ϕD:航位推算的失准角误差\phi_D:航位推算的失准角误差ϕD:航位推算的失准角误差
v~Dn=C~bn(Cbm)Tv~Dm=(I−ϕD×)Cbn(I−α×)(1+δKD)vDm≈vDn−(ϕD×)CbnvDm−Cbn(α×)vDm+CbnδKDvDm=vDn+vDn×ϕD+Cbn(vDm×)α+CbnvDmδKD\tilde v^n_D=\tilde C^n_b(C^m_b)^T\tilde v^m_D=(I-\phi_D×)C^n_b(I-\alpha×)(1+\delta K_D)v^m_D\\ \approx v^n_D-(\phi_D×)C^n_bv^m_D-C^n_b(\alpha ×)v_D^m+C^n_b\delta K_D v^m_D\\ =v^n_D+v^n_D×\phi_D+C^n_b(v^m_D×)\alpha+C^n_bv^m_D\delta K_Dv~Dn=C~bn(Cbm)Tv~Dm=(I−ϕD×)Cbn(I−α×)(1+δKD)vDm≈vDn−(ϕD×)CbnvDm−Cbn(α×)vDm+CbnδKDvDm=vDn+vDn×ϕD+Cbn(vDm×)α+CbnvDmδKD
令Cbn=Cij(i,j=1,2,3);vDm=[0vD0]TC^n_b=C_{ij}(i,j=1,2,3);v^m_D=\left[\begin{matrix}0&v_D&0\end{matrix}\right]^TCbn=Cij(i,j=1,2,3);vDm=[0vD0]T,可以简化得到：
MvkD=vD[−C13C12C11−C23C22C21−C33C32C31]M_{vkD}=v_D\left[\begin{matrix} -C_{13}&C_{12}&C_{11}\\ -C_{23}&C_{22}&C_{21}\\ -C_{33}&C_{32}&C_{31} \end{matrix}\right]MvkD=vD⎣⎡−C13−C23−C33C12C22C32C11C21C31⎦⎤
KD=[αθδKDαϕ]K_D=\left[\begin{matrix}\alpha_\theta\\\delta K_D\\\alpha_\phi\end{matrix}\right]KD=⎣⎡αθδKDαϕ⎦⎤
v~Dn=vDn+vDn×ϕD+vD[−C13C12C11−C23C22C21−C33C32C31][αθδKDαϕ]=vDn+vDn×ϕD+MvkDKD\tilde v^n_D=v^n_D+v^n_D×\phi_D+v_D\left[\begin{matrix} -C_{13}&C_{12}&C_{11}\\ -C_{23}&C_{22}&C_{21}\\ -C_{33}&C_{32}&C_{31} \end{matrix}\right] \left[\begin{matrix}\alpha_\theta\\\delta K_D\\\alpha_\phi\end{matrix}\right]\\ =v^n_D+v^n_D×\phi_D+M_{vkD}K_Dv~Dn=vDn+vDn×ϕD+vD⎣⎡−C13−C23−C33C12C22C32C11C21C31⎦⎤⎣⎡αθδKDαϕ⎦⎤=vDn+vDn×ϕD+MvkDKD
里程仪速度误差方程：
δvDn=v~Dn−vDn=vDn×ϕD+MvkKD\delta v^n_D=\tilde v^n_D-v^n_D=v^n_D×\phi_D+M_{vk}K_DδvDn=v~Dn−vDn=vDn×ϕD+MvkKD
航位推算位置方程下：
MpvD=[01/RMhD0secLD/RNhD00001]M_{pvD}=\left[\begin{matrix} 0&1/R_{MhD}&0\\ secL_D/R_{NhD}&0&0\\ 0&0&1 \end{matrix}\right]MpvD=⎣⎡0secLD/RNhD01/RMhD00001⎦⎤
MppD=[00−vDN/RMhD2vDEsecLDtanLD/RNhD0−vDEsecL/RNhD2000]M_{ppD}=\left[\begin{matrix} 0&0&-v_{DN}/R^2_{MhD}\\ v_{DE}secL_D tanL_D/R_{NhD}&0&-v_{DE}secL/R^2_{NhD}\\ 0&0&0 \end{matrix}\right]MppD=⎣⎡0vDEsecLDtanLD/RNhD0000−vDN/RMhD2−vDEsecL/RNhD20⎦⎤
δP˙D=MpvDδvDn+MppDδPD\delta \dot P_D=M_{pvD}\delta v^n_D+M_{ppD}\delta P_DδP˙D=MpvDδvDn+MppDδPD
得到航位推算误差方程为：
δp˙D=MpvD(vDn×ϕD+MvkKD)+MppDδPD=MpaDϕD+MpkDKD+MppDδPDMpaD=MpvD(vDn×);MpkD=MpvDMvkD\delta \dot p_D=M_{pvD}(v^n_D×\phi_D+M_{vk}K_D)+M_{ppD}\delta P_D=M_{paD}\phi_D+M_{pkD}K_D+M_{ppD}\delta P_D\\ M_{paD}=M_{pvD}(v^n_D×);M_{pkD}=M_{pvD}M_{vkD}δp˙D=MpvD(vDn×ϕD+MvkKD)+MppDδPD=MpaDϕD+MpkDKD+MppDδPDMpaD=MpvD(vDn×);MpkD=MpvDMvkD
航位推算姿态误差方程为：
ϕ˙D=MaaD′ϕD+MavD(vDn×ϕD+MvkDKD)+MapDδpD−Cbnξb=MaaDϕD+MakDKD+MapDδPD−CbnξbMaaD=MaaD′+MavD(vDn×),MakD=MavDMvkDMaaD′=([0wiecosLDwiesinLD]+[−vDN/RMhDvDE/RNhDvDEtanLD/RNhD])×,MavD=[0−1/RMhD01/RNhD00tanLD/RNhD00]MapD=[000−wiesinLD00wiecosLD+vDEsec2LD/RN00]\dot \phi_D=M'_{aaD}\phi_D+M_{avD}(v^n_D×\phi_D+M_{vkD}K_D)+M_{apD}\delta p_D-C^n_b\xi^b\\ =M_{aaD}\phi_D+M_{akD}K_D+M_{apD}\delta P_D-C^n_b\xi^b\\ M_{aaD}=M_{aaD}'+M_{avD}(v^n_D×),M_{akD}=M_{avD}M_{vkD}\\ M'_{aaD}=(\left[\begin{matrix} 0\\w_{ie}cosL_D\\w_{ie}sinL_D \end{matrix}\right]+\left[\begin{matrix} -v_{DN}/R_{MhD}\\v_{DE}/R_{NhD}\\v_{DE}tanL_D/R_{NhD} \end{matrix}\right])×,M_{avD}= \left[\begin{matrix} 0&-1/R_{MhD}&0\\1/R_{NhD}&0&0\\tanL_D/R_{NhD}&0&0 \end{matrix}\right]\\ M_{apD}=\left[\begin{matrix} 0&0&0\\ -w_{ie}sinL_D&0&0\\ w_{ie}cosL_D+v_{DE}sec^2L_D/R_N&0&0 \end{matrix}\right]ϕ˙D=MaaD′ϕD+MavD(vDn×ϕD+MvkDKD)+MapDδpD−Cbnξb=MaaDϕD+MakDKD+MapDδPD−CbnξbMaaD=MaaD′+MavD(vDn×),MakD=MavDMvkDMaaD′=(⎣⎡0wiecosLDwiesinLD⎦⎤+⎣⎡−vDN/RMhDvDE/RNhDvDEtanLD/RNhD⎦⎤)×,MavD=⎣⎡01/RNhDtanLD/RNhD−1/RMhD00000⎦⎤MapD=⎣⎡0−wiesinLDwiecosLD+vDEsec2LD/RN000000⎦⎤
总结：在初始误差δPD\delta P_DδPD不大时，安装偏差αθ,αϕ\alpha_\theta,\alpha_\phiαθ,αϕ里程刻度系误差δKD\delta K_DδKD陀螺仪漂移ξb\xi^bξb是主要误差来源

轨迹相似性原理

在载车行驶过程中，一般姿态角都比较小，近似有：
uU=[001]Tu_U=\left[\begin{matrix} 0& 0& 1 \end{matrix}\right]^TuU=[001]T
Cbn≈[cosϕ−sinϕ0sinϕcosϕ0001]C^n_b \approx \left[\begin{matrix} cos\phi&-sin\phi&0\\ sin\phi&cos\phi&0\\ 0&0&1 \end{matrix}\right]Cbn≈⎣⎡cosϕsinϕ0−sinϕcosϕ0001⎦⎤
vDn×uU=(cbnvDm)×uU≈[cosϕ−sinϕ0sinϕcosϕ0001][0vD0]×[001]=vD[cosϕsinϕ0]v^n_D×u_U=(c^n_bv^m_D)×u_U \approx \left[\begin{matrix} cos\phi&-sin\phi&0\\ sin\phi&cos\phi&0\\ 0&0&1 \end{matrix}\right] \left[\begin{matrix} 0\\v_D\\0 \end{matrix}\right]× \left[\begin{matrix} 0\\0\\1 \end{matrix}\right]=v_D\left[\begin{matrix} cos\phi\\sin\phi\\0 \end{matrix}\right]vDn×uU=(cbnvDm)×uU≈⎣⎡cosϕsinϕ0−sinϕcosϕ0001⎦⎤⎣⎡0vD0⎦⎤×⎣⎡001⎦⎤=vD⎣⎡cosϕsinϕ0⎦⎤
矢量误差为ϕD=[ϕDEϕDNϕDU]T\phi_D=\left[\begin{matrix} \phi_{DE}&\phi_{DN}&\phi_{DU}\end{matrix}\right]^TϕD=[ϕDEϕDNϕDU]T
近似认为ϕDE≈ϕDN≈0\phi_{DE} \approx \phi_{DN} \approx 0ϕDE≈ϕDN≈0,可得：
v~Dn≈vDn+vDn×[00ϕDU]+vD[00cosϕ00sinϕ−100][αθαrαϕ]+vDnδKD=vDn+ϕDUvDn×uU+aϕvD×uU−aθvDuU+vDnθKD=[I−(ϕDU+aϕ)uU×]vDn+δKDvDn−aθvDuU≈(1+δKD)[I−(ϕDU+αϕ)uU×]vDn−αθvDuU\tilde v^n_D \approx v^n_D+v^n_D×\left[\begin{matrix} 0\\0\\\phi_{DU} \end{matrix}\right]+v_D\left[\begin{matrix} 0&0&cos\phi\\0&0&sin\phi\\-1&0&0\\ \end{matrix}\right]\left[\begin{matrix} \alpha_\theta\\\alpha_r\\\alpha_\phi \end{matrix}\right]+v^n_D\delta K_D\\ =v^n_D+\phi_{DU}v^n_D×u_U+a_\phi v_D×u_U-a_\theta v_D u_U+v^n_D\theta K_D\\ =[I-(\phi_{DU}+a_\phi)u_U×]v^n_D+\delta K_Dv^n_D-a_\theta v_Du_U\\ \approx (1+\delta K_D)[I-(\phi_{DU}+\alpha_\phi)u_U×]v^n_D-\alpha_\theta v_D u_Uv~Dn≈vDn+vDn×⎣⎡00ϕDU⎦⎤+vD⎣⎡00−1000cosϕsinϕ0⎦⎤⎣⎡αθαrαϕ⎦⎤+vDnδKD=vDn+ϕDUvDn×uU+aϕvD×uU−aθvDuU+vDnθKD=[I−(ϕDU+aϕ)uU×]vDn+δKDvDn−aθvDuU≈(1+δKD)[I−(ϕDU+αϕ)uU×]vDn−αθvDuU
假设αθ,ϕDU+αϕ,δKD\alpha_\theta,\phi_{DU}+\alpha_\phi,\delta K_Dαθ,ϕDU+αϕ,δKD,均为常值小量，对上式等式两端进行积分：
SDn=∫0TvDndtS~Dn=∫0Tv~DndtSD=∫0TvDdtS^n_D=\int^T_0 v^n_Ddt\\ \tilde S^n_D=\int_0^T\tilde v^n_D dt\\ S_D=\int^T_0v_D dtSDn=∫0TvDndtS~Dn=∫0Tv~DndtSD=∫0TvDdt
S~Dn=(1+δKD)[I−(ϕDU+αϕ)uU×]SDn−αθSDuU\tilde S^n_D=(1+\delta K_D)[I-(\phi_{DU}+\alpha_\phi)u_U×]S^n_D-\alpha_\theta S_D u_U\\ S~Dn=(1+δKD)[I−(ϕDU+αϕ)uU×]SDn−αθSDuU
将上式分解为水平和垂直两部分：
S~Dn=[S~DES~DNS~DU]T\tilde S^n_D=\left[\begin{matrix} \tilde S_{DE}&\tilde S_{DN}&\tilde S_{DU} \end{matrix}\right]^TS~Dn=[S~DES~DNS~DU]T
SDn=[SDESDNSDU]TS^n_D=\left[\begin{matrix} S_{DE}& S_{DN}& S_{DU} \end{matrix}\right]^TSDn=[SDESDNSDU]T
S~HDn=[S~DES~DN0]T\tilde S^n_{HD}=\left[\begin{matrix} \tilde S_{DE}&\tilde S_{DN}&0 \end{matrix}\right]^TS~HDn=[S~DES~DN0]T
SDHn=[SDESDN0]T:水平面上的投影S^n_{DH}=\left[\begin{matrix} S_{DE}& S_{DN}& 0 \end{matrix}\right]^T:水平面上的投影SDHn=[SDESDN0]T:水平面上的投影
S~DHn=(1+δKD)[I−(ϕDU+αϕ)uU×]SDHnS~DU=(1+δKD)SDU−αθSD\tilde S^n_{DH}=(1+\delta K_D)[I-(\phi_{DU}+\alpha_\phi)u_U×]S^n_{DH}\\ \tilde S_{DU}=(1+\delta K_D)S_{DU}-\alpha_\theta S_DS~DHn=(1+δKD)[I−(ϕDU+αϕ)uU×]SDHnS~DU=(1+δKD)SDU−αθSD

假设载车从某一点A开始沿某一路线，行驶一圈又回到A点，如上图在行驶路线上取一点B，其对应的计算位移为点C，做点D：AC⊥BDAC\bot BDAC⊥BD，真实位移SDHn=AB⃗S^n_{DH}=\vec {AB}SDHn=AB,计算位移S~DHn=AC⃗\tilde S^n_{DH}=\vec {AC}S~DHn=AC,转动角度为ϕDU+αϕ\phi_{DU}+\alpha_\phiϕDU+αϕ.
（1）：航位推算轨迹与真实轨迹的相似性：真实位移移动ϕDU+αϕ\phi_{DU}+\alpha_\phiϕDU+αϕ角，在扩大(1+δKD)(1+\delta K_D)(1+δKD)倍得到计算位移，S~DHn\tilde S^n_{DH}S~DHn
（2）：BD⃗\vec {BD}BD:垂直方向位移DC⃗\vec {DC}DC:水平方向位移
（3）：BC⃗\vec{BC}BC：误差向量当载车距离起始点A越远误差越大，当放回原点误差会逐渐消失
（4）：定义高度误差为：δSDU=S~DU−SDU=δKDSDU−αθSD\delta S_{DU}=\tilde S_{DU}-S_{DU}=\delta K_DS_{DU}-\alpha_\theta S_DδSDU=S~DU−SDU=δKDSDU−αθSD,当里程仪刻度系数误差较小时δKD\delta K_DδKD近似有δSDU≈−aθSD\delta S_{DU} \approx -a\theta S_DδSDU≈−aθSD

惯性/里程仪组合

在这里插入图片描述

里程仪转向与杆臂矫正
如上图：
O1为AB前轮AB中心点O_1为AB前轮AB中心点O1为AB前轮AB中心点
O2为后轮CD中心点O_2为后轮CD中心点O2为后轮CD中心点
ϕ为前轮偏转角\phi 为前轮偏转角ϕ为前轮偏转角
vD:前轮/转向轮差动式里程仪输出，点O1的与地面速度v_D:前轮/转向轮差动式里程仪输出，点O_1的与地面速度vD:前轮/转向轮差动式里程仪输出，点O1的与地面速度

里程仪输出在车体坐标系（m系）的投影为：
vDm=[vDsinϕvDcosϕ0]Tv^m_D=\left[\begin{matrix}v_Dsin\phi&v_Dcos\phi&0\end{matrix}\right]^TvDm=[vDsinϕvDcosϕ0]T
偏转角ϕ\phiϕ，右转为正,−π/2<ϕ<π/2-\pi/2<\phi<\pi/2−π/2<ϕ<π/2
rϕ=vDϕ˙r_\phi=\frac{v_D}{\dot \phi}rϕ=ϕ˙vD
ϕ=∠O1OO2=arcsindLrϕ\phi=\angle O_1OO_2=arcsin \frac{dL}{r_\phi}ϕ=∠O1OO2=arcsinrϕdL

得到前轮里程仪的输出为：
vDm=[dLϕ˙vD2−(dLϕ˙)20]Tv^m_D=\left[\begin{matrix}d_L\dot\phi&\sqrt{v^2_D-(d_L\dot \phi)^2}&0\end{matrix}\right]^TvDm=[dLϕ˙vD2−(dLϕ˙)20]T
杆臂误差为：
δlDb=[dxdy0]T\delta l^b_D=\left[\begin{matrix}dx&dy&0\end{matrix}\right]^TδlDb=[dxdy0]T
得到里程计与惯导之间的杆臂误差为：
δPDL=PINS−PD=−MpvCbnδlDb\delta P_{DL}=P_{INS}-P_D=-M_{pv}C^n_b\delta l^b_DδPDL=PINS−PD=−MpvCbnδlDb

惯导/航位推算组合模式

获得捷联惯导解算姿态更新值
使用惯导的姿态矩阵对里程仪测量进行坐标变化，获得导航坐标系下的航位推算速度
此时惯导解算与航位解算有共同的姿态阵，具有相同的失准角误差，将惯导误差与航位推算误差合并，得到状态向量：
KD,ξb,▽b：随机常值向量K_D,\xi^b,\bigtriangledown ^b：随机常值向量KD,ξb,▽b：随机常值向量
X=[ϕT(δvn)T(δp)T(δpD)T(ξb)T(▽b)TKDT]TX=\left[\begin{matrix}\phi^T&(\delta v^n)^T&(\delta p)^T&(\delta p_D)^T&(\xi^b)^T&(\bigtriangledown ^b)^T&K_D^T\end{matrix}\right]^TX=[ϕT(δvn)T(δp)T(δpD)T(ξb)T(▽b)TKDT]T
获得里程仪相对于惯组的杆臂，并矫正
以惯导解算位置和航位推算位置之差构造观测量：
Z=P~INS−δPDL−P~D=δPINS−δPDZ=\tilde P_{INS}-\delta P_{DL}-\tilde P_D=\delta P_{INS}-\delta P_DZ=P~INS−δPDL−P~D=δPINS−δPD
{X˙=FX+GWbZ=HX+Vst:F=[MaaMavMap03×3−Cbn03×303×3MvaMvvMvp03×303×3Cbn03×303×3MpvMpp03×303×303×303×3MpaD03×3MppMppD03×303×3MpkD09×21],G=[−Cbn03×303×3Cbn015×6],Wb=[WgbWgb]H=[03×6I3×3−I3×303×9]\begin{cases} \dot X=FX+GW^b\\ Z=HX+V \end{cases}\\ st:\\ F=\left[\begin{matrix} M_{aa}&M_{av}&M_{ap}&0_{3×3}&-C^n_b&0_{3×3}&0_{3×3}\\ M_{va}&M_{vv}&M_{vp}&0_{3×3}&0_{3×3}&C^n_b&0_{3×3}\\ 0_{3×3}&M_{pv}&M_{pp}&0_{3×3}&0_{3×3}&0_{3×3}&0_{3×3}\\ M_{paD}&0_{3×3}&M_{pp}&M_{ppD}&0_{3×3}&0_{3×3}&M_{pkD}\\ 0_{9×21} \end{matrix}\right],G=\left[\begin{matrix} -C^n_b&0_{3×3}\\ 0_{3×3}&C^n_b\\ 0_{15×6}\end{matrix}\right],W^b=\left[\begin{matrix} W^b_g\\W^b_g \end{matrix}\right]\\ H=\left[\begin{matrix} 0_{3×6}&I_{3×3}&-I_{3×3}&0_{3×9} \end{matrix}\right]{X˙=FX+GWbZ=HX+Vst:F=⎣⎢⎢⎢⎢⎡MaaMva03×3MpaD09×21MavMvvMpv03×3MapMvpMppMpp03×303×303×3MppD−Cbn03×303×303×303×3Cbn03×303×303×303×303×3MpkD⎦⎥⎥⎥⎥⎤,G=⎣⎡−Cbn03×3015×603×3Cbn⎦⎤,Wb=[WgbWgb]H=[03×6I3×3−I3×303×9]

惯导/航位推算增量组合模式

在路况不好时，会造成里程仪打滑或者滑行，难以满足车辆转弯原理，需要抛弃这种状态不好的数据，只采用状态良好的数据，但从不良状态恢复至良好时，航位推算精度依然会损失，因此使用良好数据时间内的增量与惯导组合，降低误差。
惯导/航位推算增量组合的量测构造方法如下：

惯导解算速度与里程仪解算速度之间误差为：
z=v~INSn−v~Dn=δvINSn−δvDn≈−v~Dn×ϕ+δvINSSn−MvkDKDz=\tilde v^n_{INS}-\tilde v^n_D=\delta v^n_{INS}-\delta v^n_D \approx -\tilde v^n_D×\phi +\delta v^n_{INSS}-M_{vkD}K_Dz=v~INSn−v~Dn=δvINSn−δvDn≈−v~Dn×ϕ+δvINSSn−MvkDKD
在足够小的时间：[tj−1,tj][t_{j-1},t_j][tj−1,tj]内，惯导失准角ϕ\phiϕ视为常量，对第一步结果进行积分
Tj=tj−tj−1=1sT_j=t_j-t_{j-1}=1sTj=tj−tj−1=1s
∫z(tj)=∫vINS~(tj)−∫vD~(tj)\int_{z}(t_j)=\tilde{\int_{vINS}}(t_j)-\tilde{\int_{vD}}(t_j)∫z(tj)=∫vINS~(tj)−∫vD~(tj)
∫vD~(tj)=∫tj−1tjv~Dndt\tilde {\int_{vD}}(t_j)=\int_{t_{j-1}}^{{t_j}}\tilde v^n_Ddt∫vD~(tj)=∫tj−1tjv~Dndt
∫M(tj)=∫tj−1tjMvkDdt\int_M(t_j)=\int^{t_j}_{t_{j-1}}M_{vkD}dt∫M(tj)=∫tj−1tjMvkDdt
∫z(tj)=∫vINS~(tj)−∫vD~tj(tj)=−∫vD~(tj)×ϕ(tj)+[δvINSn(tj)Tj−δvINSn(tj−1)+δvINSn(tj)2Tj]−∫M(tj)KD\int_{z}(t_j)=\tilde{\int_{vINS}}(t_j)-\tilde{\int_{vD}}^{t_j}(t_j)\\ =-\tilde{\int_{vD}}(t_j)×\phi(t_j)+[\delta v^n_{INS}(t_j)T_j-\frac{\delta v^n_{INS}(t_{j-1})+\delta v^n_{INS}(t_{j})}{2}T_j]-\int_M(t_j)K_D∫z(tj)=∫vINS~(tj)−∫vD~tj(tj)=−∫vD~(tj)×ϕ(tj)+[δvINSn(tj)Tj−2δvINSn(tj−1)+δvINSn(tj)Tj]−∫M(tj)KD
惯导速度误差为:
δv˙INSn≈fsfn×ϕ(tj)\delta \dot v^n_{INS} \approx f^n_{sf}×\phi(t_j)δv˙INSn≈fsfn×ϕ(tj)
对等式两边积分可得:
δvINSn(tj)−δINSn(tj−1)≈∫tj−1tjfsfndt×ϕ(tj)\delta v^n_{INS}(t_{j})-\delta^n_{INS}(t_{j-1}) \approx \int^{t_j}_{t_{j-1}}f^n_{sf}dt×\phi(t_j)δvINSn(tj)−δINSn(tj−1)≈∫tj−1tjfsfndt×ϕ(tj)
将第3步求得等式带入第2步，得到：
∫z(tj)=−[Tj2∫tj−1tjfsfndt+∫vD~(tj)]×ϕ(tj)+δvINSn(tj)Tj−∫M(tj)KD\int_{z}(t_j)=-[\frac{T_j}{2}\int^{t_j}_{t_{j-1}}f^n_{sf}dt+\tilde {\int_{vD}}(t_j)]×\phi(t_j)+\delta v^n_{INS}(t_j)T_j-\int_M(t_j)K_D∫z(tj)=−[2Tj∫tj−1tjfsfndt+∫vD~(tj)]×ϕ(tj)+δvINSn(tj)Tj−∫M(tj)KD
构造惯导/航位推算增量方程为：

{X˙=FX+GWbZ(tj)=∫z(tj)=H(tj)X(tj)+V(tj)st:X=[ϕT(δvn)T(δp)T(ξb)T(▽b)TKDT]TF=[MaaMavMap03×3−Cbn03×303×3MvaMvvMvp03×303×3Cbn03×303×3MpvMpp03×303×303×303×309×21],G=[−Cbn03×303×3Cbn012×6],Wb=[WgbWgb]H(tj)=[−[Tj2∫tj−1tjfsfndt+∫vD~(tj)]×TjI3×303×9−∫M(tj)]\begin{cases} \dot X=FX+GW^b\\ Z(t_j)=\int_z(t_j)=H(t_j)X(t_j)+V(t_j) \end{cases}\\ st:\\ X=\left[\begin{matrix}\phi^T&(\delta v^n)^T&(\delta p)^T&(\xi^b)^T&(\bigtriangledown ^b)^T&K_D^T\end{matrix}\right]^T\\ F=\left[\begin{matrix} M_{aa}&M_{av}&M_{ap}&0_{3×3}&-C^n_b&0_{3×3}&0_{3×3}\\ M_{va}&M_{vv}&M_{vp}&0_{3×3}&0_{3×3}&C^n_b&0_{3×3}\\ 0_{3×3}&M_{pv}&M_{pp}&0_{3×3}&0_{3×3}&0_{3×3}&0_{3×3}\\ 0_{9×21} \end{matrix}\right],G=\left[\begin{matrix} -C^n_b&0_{3×3}\\ 0_{3×3}&C^n_b\\ 0_{12×6}\end{matrix}\right],W^b=\left[\begin{matrix} W^b_g\\W^b_g \end{matrix}\right]\\ H(t_j)=\left[\begin{matrix} -[\frac{T_j}{2}\int^{t_j}_{t_{j-1}}f^n_{sf}dt+\tilde {\int_{vD}}(t_j)]×&T_jI_{3×3}&0_{3×9}-\int_M(t_j) \end{matrix}\right]{X˙=FX+GWbZ(tj)=∫z(tj)=H(tj)X(tj)+V(tj)st:X=[ϕT(δvn)T(δp)T(ξb)T(▽b)TKDT]TF=⎣⎢⎢⎡MaaMva03×309×21MavMvvMpvMapMvpMpp03×303×303×3−Cbn03×303×303×3Cbn03×303×303×303×3⎦⎥⎥⎤,G=⎣⎡−Cbn03×3012×603×3Cbn⎦⎤,Wb=[WgbWgb]H(tj)=[−[2Tj∫tj−1tjfsfndt+∫vD~(tj)]×TjI3×303×9−∫M(tj)]