GPS/INS组合导航

Published: 2020-01-16 | Lastmod: 2020-02-11

涉及到卡尔曼滤波，最重要的是想清楚待估计的状态量，并列出动态方程、观测方程。

关于KF与Error-State KF，请参考Bayesian Filter。

常用符号 #

待估参数：

姿态角(roll, pitch, yaw)：

\[ (roll, pitch, yaw) \]

地理位置(latitude, longitude, height)：

\[ ( B , L , h) \]

NED系速度：

\[ (v_N, v_E, v_D) \]

子午圈、卯酉圈曲率半径：

\[ M=\frac{a(1-e^2)}{(1-e^2\sin^2 B )^{3/2}} \\[2mm] N=\frac{a}{(1-e^2\sin^2 B )^{1/2}} \]

角速度\(\omega_e=15.04107deg/h\)：

\[ \begin{aligned} \omega_{ie}^n &= \begin{pmatrix} \omega_e\cos B &0 &-\omega_e\sin B \end{pmatrix}^T \\[2mm] \omega_{en}^n &= \begin{pmatrix} \dot{ L }\cos B \\ -\dot{ B } \\ -\dot{ L }\sin B \end{pmatrix} =\begin{pmatrix} v_e/(N+h) \\ -v_N/(M+h) \\ -v_E\tan B /(N+h) \end{pmatrix} \end{aligned} \]

重力更新：

\[ g=9.7803267714\times\left(1-2\frac{h}{a}\right)\times\frac{1+0.001931851\sin^2L}{(1-0.006694379\sin^2L)^{1/2}} \]

旋转矩阵（r/p/y for roll/pitch/yaw）：

\[ C_b^n = \begin{pmatrix} \cos(y)*\cos(r) + \sin(p)*\sin(y)*\sin(r) &\cos(r)*\sin(p)*\sin(y) - \cos(y)*\sin(r) &\cos(p)*\sin(y) \\ \cos(p)*\sin(r) &\cos(p)*\cos(r) &-\sin(p) \\ \cos(y)*\sin(p)*\sin(r) - \cos(r)*\sin(y) &\sin(y)*\sin(r) + \cos(y)*\cos(r)*\sin(p) &\cos(p)*\cos(y) \end{pmatrix} \]

动态方程 #

微分方程

速度微分方程：

\[ \dot{v_n} = C_b^n \cdot f^b + g_l^n - 2\omega_{ie}^n \times v^n - \omega_{en}^n \times v^n \]

位置微分方程：

\[ \dot{r^n}=\begin{pmatrix} \dot{ B } \\ \dot{ L } \\ \dot{h}\end{pmatrix} =\begin{pmatrix} \frac{1}{M+h} &0 &0 \\ 0 &\frac{1}{(N+h)\cos B } &0 \\ 0 &0 &-1\end{pmatrix} \begin{pmatrix} v_N \\ v_E \\ v_D \end{pmatrix} \]

姿态微分方程：

\[ \begin{aligned} \dot{q_b^n}&=\frac{1}{2}q_b^n\otimes\omega_{nb}^b \\[2mm] &=\frac{1}{2}q_b^n\otimes\left( \omega_{ib}^b - (C_b^n)^T \omega_{in}^n \right) \\[2mm] &=\frac{1}{2}q_b^n\otimes\left( \omega_{ib}^b - (C_b^n)^T (\omega_{ie}^n+\omega_{en}^n) \right) \end{aligned} \]

等价于：

\[ \dot{C_b^n}=C_b^n(\omega_{nb}^b\times)=C_b^n(\omega_{ib}^b\times)-(\omega_{in}^n\times)C_b^n \]

可参考 Rotation

更新方程

速度更新方程：

\[ v_k^n = v_{k-1}^n + C_{b(k-1)}^{n(k-1)} \cdot f^b(t_k) \cdot (t_k-t_{k-1}) +[g_l^n-(2\omega_{ie}^n+\omega_{en}^n)\times v^n(t_{k-1})] \cdot (t_k-t_{k-1}) \]

位置更新方程：

\[ r_k^n=r_{k-1}^n+0.5\begin{pmatrix} \frac{1}{M+h} &0 &0 \\0 &\frac{1}{(N+h)\cos B } &0 \\ 0 &0 &-1\end{pmatrix}(v_k^n+v_{k-1}^n)\Delta t \]

姿态更新方程：

\[ \omega_{nb}^b=\omega_{ib}^b-(C_b^n)^T(\omega_{ie}^n+\omega_{en}^n) \\[2mm] q_b^n(t_k)\approx \left(I+\frac{1}{2}\left(\mathcal{R(\omega_{nb}^b)}\cdot(t_k-t_{k-1})\right)\right)q_b^n(t_{k-1}) \]

误差方程

速度误差方程：

\[ \begin{aligned} \delta \dot{v^n}&=F_{vr} \cdot \delta r^n + F_{vv} \cdot \delta v^n + [C_b^n \cdot f^b]\times\delta\epsilon + C_b^n \cdot \delta f^b \\[2mm] &=\begin{pmatrix} -2v_E\omega_e\cos B -\frac{v_E^2}{(N+h)\cos^2 B } &0 &\frac{-v_N v_D}{(M+h)^2}+\frac{v_E^2\tan B }{(N+h)^2} \\ 2\omega_e(v_N\cos B -v_D\sin B ) + \frac{v_E v_N}{(N+h)\cos^2 B } &0 &\frac{-v_E v_D}{(N+h)^2}-\frac{v_N v_E\tan B }{(N+h)^2} \\ 2v_E\omega_e\sin B &0 &\frac{v_E^2}{(N+h)^2}+\frac{v_N^2}{(M+h)^2}-\frac{2g}{\sqrt{MN}+h} \end{pmatrix} \cdot \delta r^n \\[2mm] &+\begin{pmatrix} \frac{v_D}{M+h} &-2\omega_e\sin B - 2\frac{v_E\tan B }{N+h} &\frac{v_N}{M+h} \\ 2\omega_e\sin B +\frac{v_E\tan B }{N+h} &\frac{v_D+v_N\tan B }{N+h} &2\omega_e\cos B +\frac{v_E}{N+h} \\ -2\frac{v_N}{M+h} &-2\omega_e\cos B -2\frac{v_E}{N+h} &0 \end{pmatrix}\cdot \delta v^n + [C_b^n \cdot f^b]\times\delta\epsilon + C_b^n \cdot \delta f^b \end{aligned} \]

位置误差方程：

\[ \begin{aligned} \delta \dot{r^n} &= F_{rr} \delta r^n + F_{rv} \delta v^n \\[2mm] &=\begin{pmatrix} 0 &0 &\frac{-v_N}{(M+h)^2} \\ \frac{v_E\sin B }{(N+h)\cos^2 B } &0 &\frac{-v_E}{(N+h)\cos^2 B } \\0 &0 &0 \end{pmatrix}\delta r^n +\begin{pmatrix} \frac{1}{M+h} &0 &0 \\ 0 &\frac{1}{(N+h)\cos B } &0 \\ 0 &0 &-1 \end{pmatrix} \delta v^n \end{aligned} \]

姿态误差方程：

\[ \begin{aligned} \dot{\delta\epsilon^n} &= F_{er}\delta r^n + F_{ev} \delta v^n -(\omega_{in}^n\times)\delta\epsilon^n -C_b^n\delta\omega_{ib}^b \\[2mm] &=\begin{pmatrix} -\omega_e\sin B &0 &\frac{-v_E}{(N+h)^2} \\ 0 &0 &\frac{v_N}{(M+h)^2} \\ -\omega_e\cos B -\frac{v_E}{(N+h)\cos^2 B } &0 &\frac{v_E\tan B }{(N+h)^2} \end{pmatrix} \delta r^n + \begin{pmatrix} 0 &\frac{1}{N+h} &0 \\ \frac{-1}{M+h} &0 &0 \\ 0 &\frac{-\tan B }{N+h} &0\end{pmatrix}\delta v^n -(\omega_{in}^n\times)\delta\epsilon^n -C_b^n\delta\omega_{ib}^b \\[2mm] \end{aligned} \]

整合在一起，可得到状态方程：

\[ \begin{pmatrix} \delta \dot{r^n} \\ \delta \dot{v^n} \\ \delta \dot{\epsilon^n} \end{pmatrix} =\begin{pmatrix} F_{rr} &F_{rv} &0 \\ F_{vr} &F_{vv} &[(C_b^n\cdot f^b)]\times \\ F_{er} &F_{ev} &-(\omega_{in}^n\times) \end{pmatrix} \begin{pmatrix} \delta r^n \\ \delta v^n \\ \delta \epsilon^n \end{pmatrix} +\begin{pmatrix} 0 &0 \\ C_b^n &0 \\ 0 &-C_b^n \end{pmatrix} \begin{pmatrix} \delta f^b \\ \delta \omega_{ib}^b \end{pmatrix} \]

若对陀螺仪、加速度计继续进行建模，分为白噪声的观测噪声，与随机游走的零偏之和，则：

\[ \delta \omega_{ib}^b = b_g + \omega_g \\ \dot{b_g} = \omega_{bg} \\[2mm] \delta f^b = b_a + \omega_a \\ \dot{b_a} = \omega_{ba} \]

则可以得到15维的状态方程：

\[ \begin{pmatrix} \delta \dot{r^n} \\ \delta \dot{v^n} \\ \delta \dot{\epsilon^n} \\ \dot{b_g} \\ \dot{b_a} \end{pmatrix} =\begin{pmatrix} F_{rr} &F_{rv} &0 &0 &0 \\ F_{vr} &F_{vv} &[(C_b^n\cdot f^b)]\times &0 &C_b^n \\ F_{er} &F_{ev} &-(\omega_{in}^n\times) &-C_b^n &0 \\ 0 &0 &0 &0 &0 \\ 0 &0 &0 &0 &0 \end{pmatrix} \begin{pmatrix} \delta r^n \\ \delta v^n \\ \delta \epsilon^n \\ b_g \\ b_a \end{pmatrix} +\begin{pmatrix} 0 &0 &0 &0 \\ 0 &C_b^n &0 &0 \\ -C_b^n &0 &0 &0 \\ 0 &0 &1 &0 \\ 0 &0 &0 &1 \end{pmatrix} \begin{pmatrix} \omega_g \\ \omega_a \\ \omega_{bg} \\ \omega_{ba} \end{pmatrix} \]

计算协方差矩阵时，采用如下公式：

\[ F_k \approx I + F_t \delta t + \frac{1}{2}(F_t \delta t)^2 \\[2mm] G_k \approx (F_k G) Q (F_k G)^T \delta t \]

观测方程 #

GPS

零速检测

NHC约束

速度约束

特殊处理 #

杆臂补偿

初始化

Next: Git
Previous: ROS Integration