031误差方程整理

最新推荐文章于 2025-09-22 16:07:20 发布

原创最新推荐文章于 2025-09-22 16:07:20 发布 · 2.1k 阅读

11 ·

CC 4.0 BY-SA版权

文章标签：

#ins #误差方程

惯性导航专栏收录该内容

36 篇文章

订阅专栏

本文深入探讨了惯性导航系统中的姿态误差、速度误差及位置误差的数学模型，详细阐述了各误差项的组成及相互关系，为理解惯导系统的工作原理提供了理论依据。

1、姿态误差

$\dot \phi = M_{aa}\phi + M_{av}\delta v^n + M_{ap}\delta p - \omega_{ibx}^{b} C_b^n \delta K_{Gx} - \omega_{iby}^{b} C_b^n \delta K_{Gy} - \omega_{ibz}^{b} C_b^n \delta K_{Gz} - C_n^b \varepsilon^b$
$\begin{bmatrix} M_{aa} & M_{av} & M_{ap} & -C_b^n & O_{3×3} \end{bmatrix} \begin{bmatrix} \phi^T \\ \\ (\delta v^n)^T \\ \\ (\delta p^n)^T \\ \\ (\varepsilon^b)^T \\ \\ (\nabla^b)^T \\ \end{bmatrix} - \omega_{ibx}^{b} C_b^n \delta K_{Gx} - \omega_{iby}^{b} C_b^n \delta K_{Gy} - \omega_{ibz}^{b} C_b^n \delta K_{Gz}$
$\begin{bmatrix} M_{aa} & M_{av} & M_{ap} & -C_b^n & O_{3×3} \end{bmatrix} \begin{bmatrix} \phi^T \\ \\ (\delta v^n)^T \\ \\ (\delta p^n)^T \\ \\ (\varepsilon^b)^T \\ \\ (\nabla^b)^T \\ \end{bmatrix} - C_b^n W_g^b$

其中：
$M_{aa}=-(\omega_{in}^n×),\space\space M_{av}= \begin{bmatrix} 0 & -1/R_{Mh} & 0 \\ \\ 1/R_{Nh} & 0 & 0 \\ \\ tanL/R_{Nh} & 0 & 0\\ \end{bmatrix} ,\space\space M_{ap}=M_1+M_2$
$M_1 = \begin{bmatrix} 0 & 0 & 0 \\ \\ -\omega_{ie}sinL & 0 & 0 \\ \\ \omega_{ie}cosL & 0 & 0 \\ \end{bmatrix} , \space\space M_2 = \begin{bmatrix} 0 & 0 & v_N R_{Mh}^2 \\ \\ 0 & 0 & -v_E/R_{Nh}^2 \\ \\ v_Esec^2L/R_{Nh} & 0 & -v_E tanL/R_{Nh}^2 \\ \end{bmatrix}$
$W_g^b = \omega_{ibx}^{b} \delta K_{Gx} + \omega_{iby}^{b} \delta K_{Gy} + \omega_{ibz}^{b} \delta K_{Gz}$

2、速度误差

$\delta \dot v^n = M_{va}\phi + M_{vv}\delta v^n + M_{vp}\delta p + f_{sfx}^{b} C_b^n \delta K_{Ax} + f_{sfy}^{b} C_b^n \delta K_{Ay} + f_{sfz}^{b} C_b^n \delta K_{Az} + C_n^b \nabla^b$

$\begin{bmatrix} M_{va} & M_{vv} & M_{vp} & O_{3×3} & C_b^n \end{bmatrix} \begin{bmatrix} \phi^T \\ \\ (\delta v^n)^T \\ \\ (\delta p^n)^T \\ \\ (\varepsilon^b)^T \\ \\ (\nabla^b)^T \\ \end{bmatrix} + f_{sfx}^{b} C_b^n \delta K_{Ax} + f_{sfy}^{b} C_b^n \delta K_{Ay} + f_{sfz}^{b} C_b^n \delta K_{Az}$
$\begin{bmatrix} M_{va} & M_{vv} & M_{vp} & O_{3×3} & C_b^n \end{bmatrix} \begin{bmatrix} \phi^T \\ \\ (\delta v^n)^T \\ \\ (\delta p^n)^T \\ \\ (\varepsilon^b)^T \\ \\ (\nabla^b)^T \\ \end{bmatrix} + C_b^n W_a^b$
其中：
$M_{va} = (f_{sf}^n×)$
$M_{vv} = (v^n×)M_{av} - ((2\omega_{ie}^n+\omega_{en}^n)×)$
$M_{vp} = (v^n×)(2M_1+M_2)+M_3$
$W_a^b= f_{sfx}^{b} \delta K_{Ax} + f_{sfy}^{b} \delta K_{Ay} + f_{sfz}^{b} \delta K_{Az}$

3、位置误差

$\delta \dot p = M_{pv}\delta v^n + M_{pp}\delta p$

$\begin{bmatrix} O_{3×3} & M_{pv} & M_{vp} & O_{3×3} & O_{3×3} \end{bmatrix} \begin{bmatrix} \phi^T \\ \\ (\delta v^n)^T \\ \\ (\delta p^n)^T \\ \\ (\varepsilon^b)^T \\ \\ (\nabla^b)^T \\ \end{bmatrix}$

其中：
$M_{pv}= \begin{bmatrix} 0 & 1/R_{Mh} & 0 \\ \\ secL/R_{Nh} & 0 & 0\\ \\ 0 & 0 & 1 \\ \end{bmatrix} , \space \space M_{pp} = \begin{bmatrix} 0 & 0 & -v_N R_{Mh}^2 \\ \\ v_E secLtanL/R_{Nh} & 0 & -v_E secL/R_{Nh}^2 \\ \\ 0 & 0 & 0 \\ \end{bmatrix}$

综上所述

$\dot X = FX + GW^b = \begin{bmatrix} M_{aa} & M_{av} & M_{ap} & -C_b^n & O_{3×3} \\ \\ M_{va} & M_{vv} & M_{vp} & O_{3×3} & C_b^n \\ \\ O_{3×3} & M_{pv} & M_{vp} & O_{3×3} & O_{3×3} \\ \\ O_{3×3} & O_{3×3} & O_{3×3} & O_{3×3} & O_{3×3} \\ \\ O_{3×3} & O_{3×3} & O_{3×3} & O_{3×3} & O_{3×3} \\ \end{bmatrix} \begin{bmatrix} \phi^T \\ \\ (\delta v^n)^T \\ \\ (\delta p^n)^T \\ \\ (\varepsilon^b)^T \\ \\ (\nabla^b)^T \\ \end{bmatrix} + \begin{bmatrix} -C_b^n & O_{3×3}\\ \\ O_{3×3} & C_b^n \\ \\ O_{3×3} & O_{3×3} \\ \\ O_{3×3} & O_{3×3} \\ \\ O_{3×3} & O_{3×3} \\ \end{bmatrix} \begin{bmatrix} W_g^b \\ \\ W_a^b \\ \end{bmatrix}$
此即为一种状态空间模型。