基于误差状态的滤波分析
根据《惯导解算原理——捷联惯导更新算法及误差分析》分析，已经得到了三个误差方程：
姿态误差方程
\[ \begin{aligned} \dot{\phi} = \phi \times \omega_{in}^n - \delta \omega_{ib}^n \end{aligned} \]
速度误差方程
\[ \begin{aligned} \delta \dot{V}^n = \delta f^n + (f^n \times)(\phi) \end{aligned} \]
位置误差方程
\[ \begin{aligned} \delta \dot{P} = \delta V \end{aligned} \]
现在，对上面的方程进行展开
姿态误差方程（矩阵形式）
这里姿态误差取全局误差角\(\phi=[\phi_E , \phi_N, \phi_U]\)，表示的是从 n 系至 n'系的等效旋转矢量，即 n'系到 n系的旋转变换
由于:
\[ \begin{aligned} \phi=\begin{bmatrix} \phi_E & \phi_N & \phi_U \end{bmatrix}^T \end{aligned} \]
\[ \begin{aligned} \omega_{ie}^n=\begin{bmatrix} 0 & \omega_{ie} \cos L & \omega_{ie} \cos L \end{bmatrix}^T \end{aligned} \]
\[ \begin{aligned} \delta \omega_{ib}^n= C_b^n \begin{bmatrix} \epsilon_x \\ \epsilon_y \\ \epsilon_z \end{bmatrix} \end{aligned} \]
其中,
- \(\omega_{ie}\)为地球自转角速度
- \(\epsilon\)为陀螺仪bias
因此，姿态误差方程\(\dot{\phi}=\phi \times \omega_{ie}^n - \delta \omega_{ib}^n\)可写成矩阵形式：
\[ \begin{aligned} \begin{bmatrix} \dot{\phi}_E \\ \dot{\phi}_N \\ \dot{\phi}_U \end{bmatrix} = \begin{bmatrix} 0 & -\phi_U & \phi_N \\ \phi_U & 0 & -\phi_E \\ -\phi_N & \phi_E & 0 \end{bmatrix} \begin{bmatrix} 0 \\ \omega_{ie} \cos L \\ \omega_{ie} \sin L \end{bmatrix} - C_b^n \begin{bmatrix} \epsilon_x \\ \epsilon_y \\ \epsilon_z \end{bmatrix} \end{aligned} \]
速度误差方程（矩阵形式）
由于：
\[ f^n=[f_E,f_N,f_U]^T \]
且比力误差为加速度计bias
\[ \begin{aligned} \delta f^n=C_b^n \begin{bmatrix} \triangledown_x \\ \triangledown_y \\ \triangledown_z \end{bmatrix} \end{aligned} \]
其中，\(\triangledown\)为加速度计的bias
因此，速度误差方程\(\delta \dot{V}=f^n \times \phi + \delta f^n\)可写成矩阵形式如下：
\[ \begin{aligned} \begin{bmatrix} \delta \dot{V}_E \\ \delta \dot{V}_N \\ \delta \dot{V}_U \end{bmatrix} = \begin{bmatrix} 0 & -f_U & f_N \\ f_U & 0 & -f_E \\ -f_N & f_E & 0 \end{bmatrix} \begin{bmatrix} \phi_E \\ \phi_N \\ \phi_U \end{bmatrix} +C_b^n \begin{bmatrix} \triangledown_x \\ \triangledown_y \\ \triangledown_z \end{bmatrix} \end{aligned} \]
位置误差方程（矩阵形式）
直接把位置误差方程\(\delta \dot{P}=\delta V\)写成矩阵形式:
\[ \begin{aligned} \begin{bmatrix} \delta \dot{P}_E \\ \delta \dot{P}_N \\ \delta \dot{P}_U \end{bmatrix} = \begin{bmatrix} \delta V_E \\ \delta V_N \\ \delta V_U \end{bmatrix} \end{aligned} \]
状态方程
状态方程基本形式：
\[ \dot{X}=F_tX+B_tW \]
对于非线性系统,矩阵\(F_t\)是误差状态导数对误差状态向量求偏导
取状态向量:
\[ \begin{aligned} X= \begin{bmatrix} \delta P_{3x1} \\ \delta V_{3x1} \\ \phi_{3x1} \\ \epsilon_{3x1} \\ \triangledown_{3x1} \end{bmatrix}_{15x1} \end{aligned} \]
其中，
- \(\delta P\)表示enu位置误差量
- \(\delta V\)表示enu速度误差量
- \(\phi\) 表示导航坐标系n系与计算坐标系n'系的等效旋转矢量偏差
- \(\epsilon\)表示陀螺仪bias
- \(\triangledown\)表示加速度计bias
根据前面推导的误差状态方程，可以得到矩阵\(F_t\):
$$ \[\begin{aligned} F_t &= \begin{bmatrix} \frac{\partial \delta \dot{P}}{\partial \delta P} & \frac{\partial \delta \dot{P}}{\partial \delta V} & \frac{\partial \delta \dot{P}}{\partial \delta \phi} & \frac{\partial \delta \dot{P}}{\partial \delta \epsilon} & \frac{\partial \delta \dot{}}{\partial \delta \triangledown} \\ \frac{\partial \delta \dot{V}}{\partial \delta P} & \frac{\partial \delta \dot{V}}{\partial \delta V} & \frac{\partial \delta \dot{V}}{\partial \delta \phi} & \frac{\partial \delta \dot{V}}{\partial \delta \epsilon} & \frac{\partial \delta \dot{V}}{\partial \delta \triangledown} \\ \frac{\partial \delta \dot{\phi}}{\partial \delta P} & \frac{\partial \delta \dot{\phi}}{\partial \delta V} & \frac{\partial \delta \dot{\phi}}{\partial \delta \phi} & \frac{\partial \delta \dot{\phi}}{\partial \delta \epsilon} & \frac{\partial \delta \dot{\phi}}{\partial \delta \triangledown} \\ \frac{\partial \delta \dot{\epsilon}}{\partial \delta P} & \frac{\partial \delta \dot{\epsilon}}{\partial \delta V} & \frac{\partial \delta \dot{\epsilon}}{\partial \delta \phi} & \frac{\partial \delta \dot{\epsilon}}{\partial \delta \epsilon} & \frac{\partial \delta \dot{\epsilon}}{\partial \delta \triangledown} \\ \frac{\partial \delta \dot{\triangledown}}{\partial \delta P} & \frac{\partial \delta \dot{\triangledown}}{\partial \delta V} & \frac{\partial \delta \dot{\triangledown}}{\partial \delta \phi} & \frac{\partial \delta \dot{\triangledown}}{\partial \delta \epsilon} & \frac{\partial \delta \dot{\triangledown}}{\partial \delta \triangledown} \end{bmatrix} \\ &= \begin{bmatrix} 0_{3x3} & I_{3x3} & 0_{3x3} & 0_{3x3} & 0_{3x3} \\ 0_{3x3} & 0_{3x3} & F_{23} & 0_{3x3} & C_b^n \\ 0_{3x3} & 0_{3x3} & F_{33} & -C_b^n & 0_{3x3} \\ & & 0_{3x15} & & \\ & & 0_{3x15} & & \end{bmatrix} \end{aligned}\]$$
\[ \begin{aligned} F_{23}= \begin{bmatrix} 0 & -f_U & f_N \\ f_U & 0 & -f_E \\ -f_N & f_E & 0 \end{bmatrix} \end{aligned} \]
\[ \begin{aligned} F_{33}= \begin{bmatrix} 0 & \omega_{ie} \sin L & -\omega_{ie} \cos L \\ -\omega_{ie} \sin L & 0 & 0 \\ \omega_{ie} \cos L & 0 & 0 \end{bmatrix} \end{aligned} \]
取噪声向量\(W\):
\[ \begin{aligned} W&= \begin{bmatrix} W_{gyro} \\ W_{acc} \end{bmatrix} \\ &= \begin{bmatrix} w_{gx} \\ w_{gy} \\ w_{gz} \\ w_{ax} \\ w_{ay} \\ w_{az} \end{bmatrix} \end{aligned} \]
其中，
- \(W_{gyro}\)是陀螺仪噪声
- \(W_{acc}\)是加速度计噪声
由于噪声是跟bias通一个通道引入的，因此矩阵\(B_t\)可以直接从\(F_t\)矩阵的后两列取，也可以直接算:
\[ \begin{aligned} B_t= \begin{bmatrix} 0_{3x3} & 0_{3x3} \\ 0_{3x3} & C_b^n \\ -C_b^n & 0_{3x3} \\ 0_{6x3} & 0_{6x3} \end{bmatrix} \end{aligned} \]
观测方程（基于点云配准获得观测）
观测方程基本形式：
这个方程其实是利用预测的状态向量X来计算出一个预测的Y，后面再根据真实的观测Y与这个预测的Y做差，求得差值
\[ Y=G_tX+C_tN \]
由于这是观测，因此\(G_t\)矩阵不是偏导
假设观测量有位置和失准角，因此取观测向量\(Y\):
\[ \begin{aligned} Y= \begin{bmatrix} \delta P \\ \phi \end{bmatrix} \end{aligned} \]
又由于
\[ \begin{aligned} X= \begin{bmatrix} \delta P_{3x1} \\ \delta V_{3x1} \\ \phi_{3x1} \\ \epsilon_{3x1} \\ \triangledown_{3x1} \end{bmatrix}_{15x1} \end{aligned} \]
观测方程中的N为观测噪声：
\[ \begin{aligned} N &= \begin{bmatrix} n_{P_{3x1}} \\ n_{\phi_{3x1}} \end{bmatrix} \\ &= \begin{bmatrix} n_{P_E} \\ n_{P_N} \\ n_{P_U} \\ n_{\phi_E} \\ n_{\phi_N} \\ n_{\phi_U} \end{bmatrix} \end{aligned} \]
因此有：
\[ \begin{aligned} G_t = \begin{bmatrix} I_{3x3} & 0_{3x3} & 0_{3x3} & 0_{3x6} \\ 0_{3x3} & 0_{3x3} & I_{3x3} & 0_{3x6} \end{bmatrix} \end{aligned} \]
\[ \begin{aligned} C_t= \begin{bmatrix} I_{3x3} & 0_{3x3} \\ 0_{3x3} & I_{3x3} \end{bmatrix} \end{aligned} \]
获取真实观测量Y时，需要计算失准角\(\phi\)，具体计算方法如下：
- 先计算误差矩阵\(C_n^{n'}\)
\[ \begin{aligned} & \because C_b^n=C_{n'}^n C_b^{n'} \\ & \therefore C_n^{n'} = C_b^{n'}(C_b^n)^T \end{aligned} \]
其中，
- \(C_b^{n'}\)为当前时刻位姿的预测值
- \(C_b^n\)为姿态矩阵的观测值（如点云匹配定位的结果）
- 根据罗德里格斯公式，获取\(\phi\)
\[ C_n^{n'} \approx I-(\phi \times) \]
观测方程（基于点云配准+轮速计获得观测）
观测方程基本形式：
这个方程其实是利用预测的状态向量X来计算出一个预测的Y，后面再根据真实的观测Y与这个预测的Y做差，求得差值
\[ Y=G_tX+C_tN \]
由于这是观测，因此\(G_t\)矩阵不是偏导
假设已知轮速计观测误差方程如下（注意不是状态误差方程，因此跟导数没关系）：
观测误差方程具体推导参见：*
\[ \delta V_b = C_n^b \delta V_n - C_n^b (V_n \times)\phi \]
有了观测误差方程后，可以对观测方程进行重写，步骤如下：
- 确定状态向量
\[ \begin{aligned} Y= \begin{bmatrix} \delta P \\ \phi \end{bmatrix} \end{aligned} \]
- 确定观测向量
\[ \begin{aligned} Y= \begin{bmatrix} \delta P \\ \delta V_b \\ \phi \end{bmatrix} \end{aligned} \]
其中，
- 相比于之前的观测向量，增加了\(\delta V_b\)，表示根据当前状态预测得到的载体轮速 与 轮速计测量值的差值
误差量\(\delta V_b\)计算公式如下：
\[ \begin{aligned} \delta V_b = \tilde{V_b}-V_b=\tilde{C}_n^b \tilde{V}_n - \begin{bmatrix} 0 \\ v_m \\ 0 \end{bmatrix} \end{aligned} \]
其中，
- \(\tilde{C}_n^b\tilde{V}_n\)表示利用当前的状态值计算得到轮速的预测值
- \(V_b=V_m=[0,v_m,0]^T\)表示轮速计实际观测值(假设轮速计观测是右x-前y-上z坐标系)
- 重写观测方程
\[ Y=G_tX+C_tN \]
其中，矩阵\(G_t\)变成如下形式：
\[ \begin{aligned} G_t = \begin{bmatrix} I_{3x3} & 0_{3x3} & 0_{3x3} & 0_{3x6} \\ 0_{3x3} & C_n^b & -C_n^b(V_n \times) & 0_{3x6}\\ 0_{3x3} & 0_{3x3} & I_{3x3} & 0_{3x6} \end{bmatrix} \end{aligned} \]
与之前的\(G_t\)矩阵相比，增加了中间那行
矩阵\(C_t\)变成如下：
\[ \begin{aligned} C_t= \begin{bmatrix} I_{3x3} & 0_{3x3} & 0_{3x3}\\ 0_{3x3} & I_{3x3} & 0_{3x3} \\ 0_{3x3} & 0_{3x3} & I_{3x3} \end{bmatrix} \end{aligned} \]
观测噪声向量变成如下：
\[ \begin{aligned} N &= \begin{bmatrix} n_{P_{3x1}} \\ n_{Vb_{3x1}} \\ n_{\phi_{3x1}} \end{bmatrix} \\ &= \begin{bmatrix} n_{P_E} \\ n_{P_N} \\ n_{P_U} \\ n_{V_{bx}} \\ n_{V_{by}} \\ n_{V_{bz}} \\ n_{\phi_E} \\ n_{\phi_N} \\ n_{\phi_U} \end{bmatrix} \end{aligned} \]
如果考虑imu安装误差的情况，具体推导参见： *
观测方程（基于GPS获得位置观测）
观测方程基本形式：
\[ Y=G_tX+C_tN \]
由于这是观测，因此\(G_t\)矩阵不是偏导
假设观测量是GPS给出的位置，因此取观测向量\(Y\):
\[ \begin{aligned} Y= \begin{bmatrix} \delta P \\ \end{bmatrix} \end{aligned} \]
又由于
\[ \begin{aligned} X= \begin{bmatrix} \delta P_{3x1} \\ \delta V_{3x1} \\ \phi_{3x1} \\ \epsilon_{3x1} \\ \triangledown_{3x1} \end{bmatrix}_{15x1} \end{aligned} \]
观测方程中的N为观测噪声：
\[ \begin{aligned} N &= \begin{bmatrix} n_{P_{3x1}} \end{bmatrix} \\ &= \begin{bmatrix} n_{P_E} \\ n_{P_N} \\ n_{P_U} \end{bmatrix} \end{aligned} \]
因此有：
\[ \begin{aligned} G_t = \begin{bmatrix} I_{3x3} & 0_{3x3} & 0_{3x3} & 0_{3x6} \end{bmatrix} \end{aligned} \]
\[ \begin{aligned} C_t&= \begin{bmatrix} I_{3x3} \end{bmatrix} \\ &= \begin{bmatrix} 1 & 0 & 0 \\ 0 & 1 & 0 \\ 0 & 0 & 1 \end{bmatrix} \end{aligned} \]