惯性导航——扩展卡尔曼滤波（一）

最新推荐文章于 2024-06-30 22:54:03 发布

这匹黑马不太黑

最新推荐文章于 2024-06-30 22:54:03 发布

阅读量1.3w

点赞数 5

分类专栏：无人机文章标签：惯性导航扩展卡尔曼滤波无人机飞控

本文链接：https://blog.csdn.net/zoujiachi666/article/details/76696982

版权

无人机专栏收录该内容

4 篇文章 3 订阅

订阅专栏

相關源碼請參考開源飛控StarryPilot:https://github.com/JcZou/StarryPilot

对于无人机的惯性导航系统，系统的状态方程是非线性的，根据扩展卡尔曼滤波方程：

Predict

x^k | k - 1 P k | k - 1 = f (x^k - 1 | k - 1, u k - 1) = F k - 1 P k - 1 | k - 1 F T k - 1 + Q k - 1 (43)

$\begin{equation} \begin{split} \hat{x}_{k|k-1}&=f(\hat{x}_{k-1|k-1},u_{k-1}) \\ P_{k|k-1}&=F_{k-1}P_{k-1|k-1}F_{k-1}^T+Q_{k-1} \\ \end{split} \end{equation}$
Update

y ~ k S k K k x^k | k P k | k = z k - h (x^k | k - 1) = H k P k | k - 1 H T k + R k = P k | k - 1 H T k S - 1 k = x^k | k - 1 + K k y ~ k = (I - K k H k) P k | k - 1 (44)

$\begin{equation} \begin{split} \tilde{y}_k&=z_k-h(\hat{x}_{k|k-1}) \\ S_k&=H_kP_{k|k-1}H_k^T+R_k \\ K_k&=P_{k|k-1}H_k^TS_k^{-1} \\ \hat{x}_{k|k}&=\hat{x}_{k|k-1}+K_k\tilde{y}_k \\ P_{k|k}&=(I-K_kH_k)P_{k|k-1} \\ \end{split} \end{equation}$
其中状态和观测矩阵为状态和观测函数的雅可比矩阵：

F k - 1 H k = \partial f \partial x | x^k - 1 | k - 1, u k - 1 = \partial h \partial x | x^k | k - 1 (45)

$\begin{equation} \begin{split} F_{k-1}&=\frac{\partial f}{\partial x}|_{\hat{x}_{k-1|k-1},u_{k-1}} \\ H_{k}&=\frac{\partial h}{\partial x}|_{\hat{x}_{k|k-1}} \\ \end{split} \end{equation}$
雅可比矩阵具体的含义可以参看Wiki: 雅可比矩阵

首先需要确定 $f$ 和 $h$ 。这里介绍两种形式的状态函数，第一种是不包含哥式校正（即不考虑地球自转以及无人机绕地球的速度所产生的向心加速度），一种是包含哥式校正的。这篇文章先介绍不包含哥式校正的EKF，包含哥式校正的将在下一篇文章介绍。

首先介绍各参数定义：
$L$ :纬度，单位: $deg*10^7$
$l$ :经度，单位: $deg*10^7$
$h$ :高度，单位: $m$
$v_N$ :朝北速度，单位: $m/s$
$v_E$ :朝东速度，单位: $m/s$
$v_D$ :朝地速度，单位: $m/s$
$R_0$ :地球平均半径
$T_x$ :x转换算子，用于将南北向位移量转化为纬度变化量， $T_x=\frac{180*10^7}{\pi R_0}$ (假设在近地面飞行，忽略高度对半径的影响。如要考虑，则分母变为 $\pi(R_0+h)$ )
$T_y$ :y转换算子，用于将朝东西向位移量转化为经度变化量， $T_y=\frac{180*10^7}{\pi R_0\sin(90-L*10^{-7})}=\frac{180*10^7*\sec(L*10^{-7})}{\pi R_0}$ (同样假设在近地面飞行)
$T$ :时间间隔,单位: $s$
$a_N$ :朝北加速度,单位: $m/t^2$
$a_E$ :朝东加速度,单位: $m/t^2$
$a_D$ :朝地加速度，已做gravity corectify,即有加上重力常量g,单位: $m/t^2$

f (x^, u) = ⎡ ⎣ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ L + v N T x T l + v E T y T h - v D T v N + a N T v E + a E T v D + a D T ⎤ ⎦ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥

$f(\hat{x},u)=\begin{bmatrix} L+v_NT_xT\\ l+v_ET_yT\\ h-v_DT\\ v_N+a_NT\\ v_E+a_ET\\ v_D+a_DT \end{bmatrix}$
其中，

x^=[LlhvNvEvD] x ^ = [ L l h v N v E v D ] $\hat{x}=\begin{bmatrix}L&l&h&v_N&v_E&v_D\end{bmatrix}$ ，

u=[aNaEaD] u = [ a N a E a D ] $u=\begin{bmatrix}a_N&a_E&a_D\end{bmatrix}$ .
由于状态的观测量可以由传感器直接获取到，所以

h h $h$ 的定义如下：

h (\hat{x}) = [\begin{matrix} L \\ l \\ h \\ v_{N} \\ v_{E} \\ v_{D} \end{matrix}]

$h(\hat{x})=\begin{bmatrix} L\\ l\\ h\\ v_N\\ v_E\\ v_D \end{bmatrix}$
根据雅可比矩阵定义，计算得状态和观测矩阵如下：

F k - 1 = ⎡ ⎣ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ 1 180 T v E sin ( L * 10 - 7 ) π R 0 cos ( L * 10 - 7 ) 2 0000 010000001000 T x T 00100 0 180 * 10 7 T π R 0 cos ( L * 10 - 7 ) 0010 00 - T 001 ⎤ ⎦ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥

$F_{k-1}=\begin{bmatrix} 1 & 0 & 0 & T_xT & 0 & 0 \\ \frac{180Tv_E\sin(L*10^{-7})}{\pi R_0\cos(L*10^{-7})^2} & 1 & 0 & 0 & \frac{180*10^7T}{\pi R_0\cos(L*10^{-7})} & 0 \\ 0 & 0 & 1 & 0 & 0 & -T \\ 0 & 0 & 0 & 1 & 0 & 0 \\ 0 & 0 & 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 0 & 0 & 1 \end{bmatrix}$

H k = I 6 \times 6

$H_k=I_{6\times 6}$

Matlab仿真效果
这里写图片描述

这匹黑马不太黑

关注

5
点赞
踩
62

收藏

觉得还不错? 一键收藏
0
评论
惯性导航——扩展卡尔曼滤波（一）

对于无人机的惯性导航系统，系统的状态方程是非线性的，根据扩展卡尔曼滤波方程：Predict x^k|k−1=f(x^k−1|k−1,uk−1)Pk|k−1=Fk−1Pk−1|k−1FTk−1+Qk−1\begin{equation}\begin{split}& \hat{x}_{k|k-1}=f(\hat{x}_{k-1|k-1},u_{k-1}) \\& P_{k|k-1}=F_{k-1}
复制链接

扫一扫

专栏目录