INS 时间更新

2023-12-17 20:27:32

4 时间更新

下式是位置、速度、姿态的误差方程式，详细的推导可参考严恭敏的《捷联惯导算法与组合导航原理讲义》
$\begin{aligned} \delta \dot{\lambda} &= \frac{v_EsecLtanL}{R_{Nh}}\delta L - \frac{v_EsecL}{R^2_{Nh}}\delta h + \frac{secL}{R_{Nh}}\delta v_E\\ \delta \dot{L} &= - \frac{v_N}{R^2_{Mh}}\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_Ev_N sec^2L}{R_{Nh}}]\delta L +\frac{v_E(v_U-v_NtanL)}{R^2_{Nh}}\delta h+ \frac{v_NtanL - 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\phi_N + f_N\phi_U +\nabla_E\\ \dot{v}_N &= -v_E(2\omega_N + \frac{v_E sec^2L}{R_{Nh}})\delta L +(\frac{v_Nv_U}{R^2_{Mh}}+\frac{v^2_EtanL}{R^2_{Nh}})\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\phi_E - f_E\phi_U\\ &+ \nabla_N\\ \dot{v}_U &= -[2\omega_Uv_E+g_esin2L(\beta-4\beta_1cos2L)]\delta L -(\frac{v^2_E}{R^2_{Nh}} + \frac{v^2_N}{R^2_{Mh}} - \beta_2)\delta_h+-f_N\phi_E + f_E\phi_N + 2(\omega_N + \frac{v_E}{R_{Nh}})\delta v_E +\frac{2v_N}{R_{Mh}}\delta v_N\\ &\nabla_U\\ \dot{\phi}_E &=\frac{v_N}{R_{Mh}^2}\delta h - \frac{1}{R_{Mh}}\delta v_N+ (\omega_U+\frac{v_E tanL}{R_{Nh}})\phi_N - (\omega_N + \frac{v_E}{R_{Nh}})\phi_U - \varepsilon_E\\ \dot{\phi}_N &= - \omega_U\delta L -\frac{v_E}{R^2_{Mh}}\delta h + \frac{1}{R_{Nh}}\delta v_E -(\omega_U+\frac{v_E tanL}{R_{Nh}})\phi_E -\frac{v_N}{R_{Mh}}\phi_U-\varepsilon_N\\ \dot{\phi}_U &= (\omega_N+\frac{v_E sec^2L}{R_{Nh}})\delta L -\frac{v_EtanL}{R^2_{Nh}}\delta h+ \frac{tanL}{R_{Nh}}\delta v_E + (\omega_N+\frac{v_E}{R_{Nh}})\phi_E +\frac{v_N}{R_{Mh}}\phi_N - \varepsilon_U\\ \end{aligned}$
将上式采用矩阵的形式可以写作：
$\begin{aligned} \delta\dot{\mathbf{x}}(t) = \mathbf{F}(t)\delta \mathbf{x}(t) \end{aligned}$

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

\	$\delta L$	$\delta h$	$\delta v_E$	$\delta v_N$	$\delta v_U$	$\delta\phi_{pitch}$	$\delta\phi_{roll}$	$\delta\phi_{yaw}$
$\delta\dot{\lambda}$	$\frac{v_EsecLtanL}{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_Ev_Nsec^2L}{R_{Nh}}$	$\frac{v_Ev_U-v_Ev_ntan\lambda}{R_{Nh}^2}$	$\frac{v_NtanL-v_U}{R_{Nh}}$	$2\omega_U+\frac{v_EtanL}{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_Esec^2L}{R_{Nh}})$	$\frac{v_Nv_U}{R_{Mh}^2}+\frac{v_E^2tanL}{R_{Nh}^2}$	$2(\omega_U+\frac{v_EtanL}{R_{Nh}})$	$-\frac{v_U}{R_{Mh}}$	$-\frac{v_N}{R_{Mh}}$	$f_U$		$f_E$
$\delta\dot{v}_U$	$-2\omega_Uv_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{\phi}_E$		$\frac{v_N}{R_{Mh}^2}$		$-\frac{1}{R_{Mh}}$			$\omega_U+\frac{v_EtanL}{R_{Nh}}$	$-(\omega_N+\frac{v_E}{R_{Nh}})$
$\delta\dot{\phi}_N$	$\omega_U$	$\frac{v_E}{R_{Mh}^2}$	$\frac{1}{R_{Mh}}$			$-(\omega_U+\frac{v_EtanL}{R_{Nh}})$		$-\frac{v_N}{R_{Mh}}$
$\delta\dot{\phi}_U$	$\omega_U+\frac{v_EtanL}{R_{Nh}}$	$\frac{v_EtanL}{R_{Mh}^2}$	$\frac{tanL}{R_{Nh}}$			$\omega_N+\frac{v_E}{R_{Nh}}$	$\frac{v_N}{R_{Mh}}$	1

续表	$abias_x$	$abias_y$	$abias_z$	$gbias_x$	$gbias_y$	$gbias_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{\phi}_E$				$c_{11}$	$c_{12}$	$c_{13}$
$\delta\dot{\phi}_N$				$c_{21}$	$c_{22}$	$c_{23}$
$\delta\dot{\phi}_U$				$c_{31}$	$c_{32}$	$c_{33}$

进一步考虑加速度计和角速度计的测量噪声和加速度计零偏角速度计零偏噪声的随机模型，INS误差模型及传感器误差模型的联合形式可以进一步表达为：
$\begin{aligned} \delta\dot{\mathbf{x}}(t) = \mathbf{F}(t)\delta \mathbf{x}(t) + \mathbf{G}(t)\mathbf{w}(t) \end{aligned}$
其中，噪声向量依次为加速度计测量噪声、角速度计测量噪声、加速度计零偏噪声，角速度计零偏噪声为：
$\begin{aligned} \mathbf{w} = [\mathbf{w}^T_a, \mathbf{w}^T_g, \mathbf{w}^T_{ab}, \mathbf{w}^T_{gb}]^T \end{aligned}$
噪声向量的设计矩阵为：
$\begin{aligned} \mathbf{G}=\begin{bmatrix} 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{bmatrix} \end{aligned}$

$\mathbf{w}$ 为噪声向量。 $\mathbf{w}(t)$ 的元素为白噪声，其协方差阵为： $\begin{aligned} E[\mathbf{w}(t)\mathbf{w}(\tau)^T] = \mathbf{Q}(t)\delta(t-\tau) \end{aligned}$ 由于捷联惯导系统一般使用高频离散采样数据，因此需要将方程转化为离散的形式：
$\begin{aligned} \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{aligned}$ $\Phi_k$ 称为状态转移矩阵。

如果 $\triangle t_{k+1} = t_{k+1} -t_k$ 很小，因此在这段时间内可以认为 $F (t)$ 为常量。转移矩阵的近似计算方法如下：
$\Phi_k = exp(F(t)\triangle t_{k+1})\approx I+F(t)\triangle t_{k+1}$ 状态时间更新对应的协方差更新为：
$\begin{aligned} 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{aligned}$ 使用式子(1.6)和(1.8)即可完成时间更新。

文章来源:https://blog.csdn.net/qq_40230900/article/details/135028785
本文来自互联网用户投稿，该文观点仅代表作者本人，不代表本站立场。本站仅提供信息存储空间服务，不拥有所有权，不承担相关法律责任。如若内容造成侵权/违法违规/事实不符，请联系我的编程经验分享网邮箱：veading@qq.com进行投诉反馈，一经查实，立即删除！