滤波器分析——状态方程和观测方程

Catalogue
  1. 1. 基于误差状态的滤波分析
    1. 1.1. 姿态误差方程(矩阵形式)
    2. 1.2. 速度误差方程(矩阵形式)
    3. 1.3. 位置误差方程(矩阵形式)
    4. 1.4. 状态方程
    5. 1.5. 观测方程(基于点云配准获得观测)
    6. 1.6. 观测方程(基于点云配准+轮速计获得观测)
    7. 1.7. 观测方程(基于GPS获得位置观测)

基于误差状态的滤波分析

根据《惯导解算原理——捷联惯导更新算法及误差分析》分析,已经得到了三个误差方程:

姿态误差方程

\[ \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\),具体计算方法如下:

  1. 先计算误差矩阵\(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\)为姿态矩阵的观测值(如点云匹配定位的结果)
  1. 根据罗德里格斯公式,获取\(\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 \]

有了观测误差方程后,可以对观测方程进行重写,步骤如下:

  1. 确定状态向量

\[ \begin{aligned} Y= \begin{bmatrix} \delta P \\ \phi \end{bmatrix} \end{aligned} \]

  1. 确定观测向量

\[ \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坐标系)
  1. 重写观测方程

\[ 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} \]