INS 时间更新

4 时间更新

下式是位置、速度、姿态的误差方程式，详细的推导可参考严恭敏的《捷联惯导算法与组合导航原理讲义》
$$\begin{array}{rl}\delta \dot{\lambda }& =\frac{v_{E}secLtanL}{R_{Nh}}\delta L-\frac{v_{E}secL}{R_{Nh}^2}\delta h+\frac{secL}{R_{Nh}}\delta v_E\\ \delta \dot{L}& =-\frac{v_N}{R_{Mh}^2}\delta h+\frac{1}{R_{Mh}}\delta v_N\\ \delta \dot{h}& =\delta v_U\\ {\dot{v}}_E& =[2(v_N{\omega }_N+v_U{\omega }_U)+\frac{v_E{v}_{N}se{c}^{2}L}{R_{Nh}}]\delta L+\frac{v_E(v_U-v_{N}tanL)}{R_{Nh}^2}\delta h+\frac{v_{N}tanL-v_U}{R_{Nh}}{\delta }_{v_E}+(2{\omega }_U+\frac{v_{E}tanL}{R_{Nh}})\delta v_N-(2{\omega }_N+\frac{v_E}{R_{Nh}})\delta v_U\\ & -f_U{\varphi }_N+f_N{\varphi }_U+{\nabla }_E\\ {\dot{v}}_N& =-v_E(2{\omega }_N+\frac{v_{E}se{c}^{2}L}{R_{Nh}})\delta L+(\frac{v_N{v}_U}{R_{Mh}^2}+\frac{v_E^{2}tanL}{R_{Nh}^2})\delta h-2({\omega }_U+\frac{v_{E}tanL}{R_{Nh}})\delta v_E-\frac{v_U}{R_{Mh}}\delta v_N-\frac{v_N}{R_{Mh}}\delta v_U+f_U{\varphi }_E-f_E{\varphi }_U\\ & +{\nabla }_N\\ {\dot{v}}_U& =-[2{\omega }_U{v}_E+g_{e}sin2L(\beta -4{\beta }_{1}cos2L)]\delta L-(\frac{v_E^2}{R_{Nh}^2}+\frac{v_N^2}{R_{Mh}^2}-{\beta }_2){\delta }_h+-f_N{\varphi }_E+f_E{\varphi }_N+2({\omega }_N+\frac{v_E}{R_{Nh}})\delta v_E+\frac{2v_N}{R_{Mh}}\delta v_N\\ & {\nabla }_U\\ {\dot{\varphi }}_E& =\frac{v_N}{R_{Mh}^2}\delta h-\frac{1}{R_{Mh}}\delta v_N+({\omega }_U+\frac{v_{E}tanL}{R_{Nh}}){\varphi }_N-({\omega }_N+\frac{v_E}{R_{Nh}}){\varphi }_U-{\epsilon }_E\\ {\dot{\varphi }}_N& =-{\omega }_U\delta L-\frac{v_E}{R_{Mh}^2}\delta h+\frac{1}{R_{Nh}}\delta v_E-({\omega }_U+\frac{v_{E}tanL}{R_{Nh}}){\varphi }_E-\frac{v_N}{R_{Mh}}{\varphi }_U-{\epsilon }_N\\ {\dot{\varphi }}_U& =({\omega }_N+\frac{v_{E}se{c}^{2}L}{R_{Nh}})\delta L-\frac{v_{E}tanL}{R_{Nh}^2}\delta h+\frac{tanL}{R_{Nh}}\delta v_E+({\omega }_N+\frac{v_E}{R_{Nh}}){\varphi }_E+\frac{v_N}{R_{Mh}}{\varphi }_N-{\epsilon }_U\end{array}$$
将上式采用矩阵的形式可以写作：
$$\begin{array}{r}\delta \dot{\mathbf{x}}(t)=\mathbf{F}(t)\delta \mathbf{x}(t)\end{array}$$

其中，$\mathbf{F}$的具体形式可以参见下表。

\	$\delta \lambda$	$\delta L$	$\delta h$	$\delta v_E$	$\delta v_N$	$\delta v_U$	$\delta {\varphi }_{pitch}$	$\delta {\varphi }_{roll}$	$\delta {\varphi }_{yaw}$
$\delta \dot{\lambda }$		$\frac{v_{E}secLtanL}{R_{Nh}}$	$-\frac{v_{E}secL}{R_{Nh}^2}$	$\frac{secL}{R_{Nh}}$
$\delta \dot{L}$			$-\frac{v_n}{R_{Mh}^2}$		$\frac{1}{R_{Mh}}$
$\delta \dot{h}$						1
$\delta \dot{v_E}$		$2(v_N{\omega }_N+v_U{\omega }_U)+\frac{v_E{v}_{N}se{c}^{2}L}{R_{Nh}}$	$\frac{v_E{v}_U-v_E{v}_{n}tan\lambda }{R_{Nh}^2}$	$\frac{v_{N}tanL-v_U}{R_{Nh}}$	$2{\omega }_U+\frac{v_{E}tanL}{R_{Nh}}$	$-(2{\omega }_N+\frac{v_E}{R_{Nh}})$		$-f_U$	$f_N$
$\delta \dot{v_N}$		$-v_E(2{\omega }_N+\frac{v_{E}se{c}^{2}L}{R_{Nh}})$	$\frac{v_N{v}_U}{R_{Mh}^2}+\frac{v_E^{2}tanL}{R_{Nh}^2}$	$2({\omega }_U+\frac{v_{E}tanL}{R_{Nh}})$	$-\frac{v_U}{R_{Mh}}$	$-\frac{v_N}{R_{Mh}}$	$-f_U$		$-f_E$
$\delta {\dot{v}}_U$		$-2{\omega }_U{v}_E$	$-(\frac{v_E^2}{R_{Nh}^2}+\frac{v_N^2}{R_{Mh}^2})$	$2({\omega }_N+\frac{v_E}{R_{Nh}})$	$\frac{2v_N}{R_{Mh}}$		$-f_N$	$f_E$
$\delta {\dot{\varphi }}_E$			$\frac{v_N}{R_{Mh}^2}$		$-\frac{1}{R_{Mh}}$			${\omega }_U+\frac{v_{E}tanL}{R_{Nh}}$	$-({\omega }_N+\frac{v_E}{R_{Nh}})$
$\delta {\dot{\varphi }}_N$		${\omega }_U$	$\frac{v_E}{R_{Mh}^2}$	$\frac{1}{R_{Mh}}$			$-({\omega }_U+\frac{v_{E}tanL}{R_{Nh}})$		$-\frac{v_N}{R_{Mh}}$
$\delta {\dot{\varphi }}_U$		${\omega }_U+\frac{v_{E}tanL}{R_{Nh}}$	$\frac{v_{E}tanL}{R_{Mh}^2}$	$\frac{tanL}{R_{Nh}}$			${\omega }_N+\frac{v_E}{R_{Nh}}$	$\frac{v_N}{R_{Mh}}$	1

续表	$abia{s}_x$	$abia{s}_y$	$abia{s}_z$	$gbia{s}_x$	$gbia{s}_y$	$gbia{s}_z$
$\delta \dot{\lambda }$
$\delta \dot{L}$
$\delta \dot{H}$
$\delta {\dot{v}}_E$	$c_{11}$	$c_{12}$	$c_{13}$
$\delta {\dot{v}}_N$	$c_{21}$	$c_{22}$	$c_{23}$
$\delta {\dot{v}}_U$	$c_{31}$	$c_{32}$	$c_{33}$
$\delta {\dot{\varphi }}_E$				$-c_{11}$	$-c_{12}$	$-c_{13}$
$\delta {\dot{\varphi }}_N$				$-c_{21}$	$-c_{22}$	$-c_{23}$
$\delta {\dot{\varphi }}_U$				$-c_{31}$	$-c_{32}$	$-c_{33}$

进一步考虑加速度计和角速度计的测量噪声和加速度计零偏角速度计零偏噪声的随机模型，INS误差模型及传感器误差模型的联合形式可以进一步表达为：
$$\begin{array}{r}\delta \dot{\mathbf{x}}(t)=\mathbf{F}(t)\delta \mathbf{x}(t)+\mathbf{G}(t)\mathbf{w}(t)\end{array}$$
其中，噪声向量依次为加速度计测量噪声、角速度计测量噪声、加速度计零偏噪声，角速度计零偏噪声为：
$$\begin{array}{r}\mathbf{w}=[{\mathbf{w}}_a^T,{\mathbf{w}}_g^T,{\mathbf{w}}_{ab}^T,{\mathbf{w}}_{gb}^T{]}^T\end{array}$$
噪声向量的设计矩阵为：
$$\begin{array}{r}\mathbf{G}=\left[\begin{array}{cccc}0& 0& 0& 0\\ {\mathbf{C}}_b^n& 0& 0& 0\\ 0& {\mathbf{C}}_b^n& 0& 0\\ 0& 0& \mathbf{I}& 0\\ 0& 0& 0& \mathbf{I}\end{array}\right]\end{array}$$

$\mathbf{w}$为噪声向量。$\mathbf{w}(t)$的元素为白噪声，其协方差阵为：$$\begin{array}{r}E[\mathbf{w}(t)\mathbf{w}(\tau {)}^T]=\mathbf{Q}(t)\delta (t-\tau )\end{array}$$由于捷联惯导系统一般使用高频离散采样数据，因此需要将方程转化为离散的形式：
$$\begin{array}{r}\delta \mathbf{x}(t_{k+1})=\Phi (t_{k+1},t_k)\delta \mathbf{x}(t_k)+\mathbf{G}(\tau )\mathbf{w}(\tau )d\tau \end{array}$$${\Phi }_k$称为状态转移矩阵。

如果$△t_{k+1}=t_{k+1}-t_k$很小，因此在这段时间内可以认为$F(t)$为常量。转移矩阵的近似计算方法如下：
$${\Phi }_k=exp(F(t)△t_{k+1})\approx I+F(t)△t_{k+1}$$状态时间更新对应的协方差更新为：
$$\begin{array}{r}{Q}_{\delta \mathbf{x}(t_{k+1})}=\Phi (t_{k+1},t_k)Q_{\delta \mathbf{x}(t_k)}{\Phi }^T(t_{k+1}+\mathbf{G}(\tau )Q_{\mathbf{w}(\tau )d\tau }{\mathbf{G}}^T(\tau )\end{array}$$使用式子(1.6)和(1.8)即可完成时间更新。