卡尔曼滤波器¶

理论¶

卡尔曼滤波基于线性、高斯系统的假设，能够利用前一时刻的状态（和可能的测量值）来得到当前时刻下的状态的最优估计。算法由预测、更新（校正）两步骤组成，既能够基于状态空间模型进行状态预测，本质上又是一个数据融合算法。

概述¶

以状态空间的形式描述一个线性高斯系统，下标表示时刻：

\begin{aligned} (1) & x_{k} & = A x_{k - 1} + B u_{k} + w_{k - 1} \\ (2) & z_{k} & = H x_{k} + v_{k} \end{aligned}

其中 $A, B$ 分别表示状态转移矩阵和控制量输入矩阵， $H$ 表示状态观测矩阵； $x_{k}$ 为系统状态量矩阵， $z_{k}$ 为实测得到的状态量矩阵；引入高斯分布， $w_{k - 1}$ 表示过程噪声（理论模型与现实模型的误差）， $v_{k}$ 表示观测噪声（量测与现实数据的误差），分别满足 $P (w) \sim N (0, Q), P (v) \sim N (0, R)$ ，其中 $Q$ 表示过程噪声协方差， $R$ 表示观测噪声协方差。

为区分状态的多种表示，定义以下符号：

\begin{aligned} x_{k} : & 状 态 观 测 值 \\ {\hat{x}}_{k m e a s u r e} : & 由 观 测 初 步 得 到 的 状 态 估 计 值 \\ {\hat{x}}_{k} : & 最 优 状 态 估 计 值 ， 或 称 后 验 状 态 估 计 值 \\ {\hat{x}}_{k}^{-} : & 状 态 预 测 值 ， 或 称 先 验 状 态 估 计 值 \end{aligned}

现实中我们并不知道噪声的实际值，但我们可以通过迭代优化，逼近最优的状态估计。由 $(1)$ 式，将状态预测方程表示为：

\begin{matrix} (a) & {\hat{x}}_{k}^{-} = A {\hat{x}}_{k - 1} + B u_{k} \end{matrix}

由 $(2)$ 式，由当下的状态量观测初步得到一个状态估计：

{\hat{x}}_{k m e a s u r e} = H^{- 1} z_{k}

由于噪声的存在， ${\hat{x}}_{k}^{-}$ 和 ${\hat{x}}_{k m e a s u r e}$ 都不能准确表示当下的状态，我们采用一个朴素的思想，即对这两者进行加权平均。状态最优估计方程：

{\hat{x}}_{k} = {\hat{x}}_{k}^{-} + G ({\hat{x}}_{k m e a s u r e} - {\hat{x}}_{k}^{-})

其中系数 $G = 0$ 时，完全相信状态预测；系数 $G = 1$ 时，完全相信状态观测。记 $G = K H$ ，上式化为：

\begin{matrix} (b) & {\hat{x}}_{k} = {\hat{x}}_{k}^{-} + K (z_{k} - H {\hat{x}}_{k}^{-}) \end{matrix}

此处 $K \in [0, H^{- 1}]$ 即为卡尔曼增益，求取状态最优估计的问题转化为求取最优 $K$ 的问题。

卡尔曼增益¶

我们希望最优估计与实际值误差最小，即后验估计误差 $e_{k} = x_{k} - {\hat{x}}_{k}$ 绝对值最小。引入后验估计误差的协方差矩阵 $P_{k} = E [e_{k} e_{k}^{T}]$ ，对 $n$ 个状态变量相互独立，有：

P = [\begin{matrix} c o v (e_{1}, e_{1}) & . . . & c o v (e_{1}, e_{n}) \\ . . . & . . . & . . . \\ c o v (e_{n}, e_{1}) & . . . & c o v (e_{n}, e_{n}) \end{matrix}] = [\begin{matrix} σ_{1} e_{1}^{2} & 0 & . . . & 0 \\ 0 & σ_{2} e_{2}^{2} & . . . & 0 \\ . . . & . . . & . . . & . . . \\ 0 & 0 & . . . & σ_{n} e_{n}^{2} \end{matrix}]

卡尔曼增益最优问题可转化为求 $K_{k}$ 使得 $t r (P_{k})$ 最小，思路为对 $t r (P_{k})$ 求导并令结果为 $0$ ，进而用其他量表示 $K_{k}$ 。以下进行求解。

由 $(2) (b)$ 式，有：

\begin{aligned} {\hat{x}}_{k} & = {\hat{x}}_{k}^{-} + K (H x_{k} + v_{k} - H {\hat{x}}_{k}^{-}) \\ \Rightarrow - {\hat{x}}_{k} & = - {\hat{x}}_{k}^{-} - K H (x_{k} - {\hat{x}}_{k}^{-}) - K v_{k} \\ \Rightarrow x_{k} - {\hat{x}}_{k} & = x_{k} - {\hat{x}}_{k}^{-} - K H (x_{k} - {\hat{x}}_{k}^{-}) - K v_{k} \\ \Rightarrow e_{k} & = (I - K H) e_{k}^{-} - K v_{k} \end{aligned}

经计算得到：

\begin{matrix} (3) & P_{k} = E [e_{k} e_{k}^{T}] = P_{k}^{-} - K_{k} H P_{k}^{-} - P_{k}^{-} H^{T} K_{k}^{T} + K_{k} (H P_{k}^{-} H^{T} + R) K_{k}^{T} \end{matrix}

$t r (P_{k})$ 对 $K_{k}$ 求导，经计算得到：

\frac{\partial t r (P_{k})}{\partial K_{k}} = - 2 P_{k}^{- T} H^{T} + 2 K H P_{k}^{-} H^{T} + 2 K R ≜ 0

则卡尔曼增益为：

\begin{matrix} (c) & K_{k} = \frac{P_{k}^{-} H^{T}}{H P_{k}^{-} H^{T} + R} \end{matrix}

观察上式可知，观测噪声协方差 $R$ 越小，增益越大，越相信状态观测值； $R$ 越大，越相信状态预测值。

卡尔曼滤波¶

先验估计误差 $e_{k}^{-} = x_{k} - {\hat{x}}_{k}^{-}$ ，其协方差矩阵 $P_{k}^{-} = E [e_{k}^{-} e_{k}^{- T}]$ 。由 $(1) (a)$ 式,，有：

\begin{aligned} e_{k}^{-} & = A x_{k - 1} + B u_{k} + w_{k - 1} - A {\hat{x}}_{k - 1} - B u_{k} \\ = A e_{k - 1} + w_{k - 1} \end{aligned}

由 $(c)$ 式，为得到 $K_{k}$ ，计算先验估计协方差 $P_{k}^{-}$ ，并化简得到：

\begin{aligned} P_{k}^{-} & = E [(A e_{k - 1} + w_{k - 1}) (A e_{k - 1} + w_{k - 1})^{T}] \\ = A E [e_{k - 1} e_{k - 1}^{T}] A^{T} + E [w_{k - 1} w_{k - 1}^{T}] \\ (d) & = A P_{k - 1} A^{T} + Q \end{aligned}

需要上一时刻的后验协方差 $P_{k - 1}$ . 进一步联立 $(3) (c)$ 式，计算 $P_{k}$ ：

\begin{aligned} P_{k} = & P_{k}^{-} - K_{k} H P_{k}^{-} - P_{k}^{-} H^{T} K_{k}^{T} + \frac{P_{k}^{-} H^{T}}{H P_{k}^{-} H^{T} + R} (H P_{k}^{-} H^{T} + R) K_{k}^{T} \\ (e) & = (I - K_{k} H) P_{k}^{-} \end{aligned}

此时 $(a) \sim (e)$ 五式形成了迭代预测、更新的关系。此五式即为完整的卡尔曼滤波算法：

\begin{aligned} 预 测 ： & {\hat{x}}_{k}^{-} = A {\hat{x}}_{k - 1} + B u_{k} \\ P_{k}^{-} = A P_{k - 1} A^{T} + Q \\ 更 新 ： & K_{k} = \frac{P_{k}^{-} H^{T}}{H P_{k}^{-} H^{T} + R} \\ {\hat{x}}_{k} = {\hat{x}}_{k}^{-} + K_{k} (z_{k} - H {\hat{x}}_{k}^{-}) \\ P_{k} = (I - K_{k} H) P_{k}^{-} \end{aligned}

其中，预测部分将后验状态估计和后验协方差从 $k - 1$ 时刻推向 $k$ 时刻，形成预测；更新部分首先计算卡尔曼增益，根据该增益，参考状态预测值和观测值，得到状态后验估计，然后更新后验协方差。该过程不断循环；初始状态下，需要给出 ${\hat{x}}_{0}$ 和 $P_{0}$ 。

观测数据融合¶

由于卡尔曼滤波的本质是数据融合，在没有状态空间模型的情况下，若有两个观测器对同一个状态进行观测，产生两组数据，则它们也可以通过卡尔曼滤波的思想进行融合。此时， $A = I, B = 0, H = I$ ，算法简化为：

\begin{aligned} 预 测 ： & {\hat{x}}_{k}^{-} = D a t a 1 \\ P_{k}^{-} = P_{k - 1} + Q \\ 更 新 ： & K_{k} = P_{k | k - 1} (P_{k | k - 1} + R)^{- 1} \\ {\hat{x}}_{k} = {\hat{x}}_{k | k - 1} + K_{k} (D a t a 2 - {\hat{x}}_{k}^{-}) \\ P_{k} = (I - K_{k}) P_{k}^{-} \end{aligned}

参数调整¶

$A, B$ 及 $H$ 基于状态空间模型设置。

实际实现时，观测噪声协方差 $R$ 一般可以观测到；但过程噪声协方差 $Q$ 较难确定。根据上文，调参的一个基本原则是 $R$ 越小，越相信状态观测值； $Q$ 越小，越相信状态预测值。一般给 $Q$ 一个较小的值有助于更方便地调整 $R$ 。

调整后验估计误差协方差初值 $P_{0}$ 时，一般给一个较小的初值有利于更快收敛。运行时，若参数逐步收敛并保持为常量，则滤波器系数可以参考此结果进行调整，以期在后续在线运行时获得更好的收敛效率。

此外，可以在运行时根据模型的实际情况改变 $Q, R$ 的值。

快速开始¶

组件源码仓库地址：https://github.com/ZJU-HelloWorld/HW-Components

要在项目中使用该组件，需添加仓库内的以下文件：

Text Only
1 2 3 4	`algorithms/filter.c algorithms/filter.h tools.h system.h`

使用前准备¶

本组件涉及 CMSIS-DSP 矩阵运算等操作。

使用本组件前需要做以下准备：

添加源文件, 包含头文件路径；注意 DSP 版本须在 1.10.0 及以上
添加预处理宏以开启浮点运算单元（FPU）
在使用 STM32CubeMX 生成项目时，请在 Code Generator 界面 Enable Full Assert，来帮助断言设备驱动中的错误；在 main.c 中修改 assert_failed 函数以指示断言结果，如添加 while(1);
在 system.h 中 system options: user config 处进行系统设置

示例¶

在项目中引用头文件：

C
#include "filter.h"

实例化一个卡尔曼滤波器：

Kalman FilterExtend Kalman Filter (TODO)

C
Kf_t kf;

C
Ekf_t ekf;

指定状态维度、滤波器类型和各矩阵初值，初始化卡尔曼滤波器。

注意以下几点：

若有状态空间模型，需要 ${\hat{x}}_{0}, P_{0}, Q, R, A, B, H$ 七组矩阵数据；若为 2 传感器数据融合，需要 ${\hat{x}}_{0}, P_{0}, Q, R$ 四组矩阵数据。
注意矩阵维度，注意协方差矩阵均为对角阵
矩阵数据的排列顺序：从左至右，从上到下。

KF_MODEL_BASEDKF_SENSOR_FUSION

C
float x[2] = {0, 1};
float p[4] = {1, 0, 0, 1};
float q[4] = {1.00, 0, 0, 1.00};
float r[4] = {0.1, 0, 0, 0.1};
float a[4] = {1, 1, 0, 1};
float b[4] = {0, 0, 0, 0};
float h[4] = {1, 0, 0, 1};
KfInit(&kf, 2, KF_MODEL_BASED, x, p, q, r, a, b, h);

C
float x[2] = {0, 1};
float p[4] = {1, 0, 0, 1};
float q[4] = {1.00, 0, 0, 1.00};
float r[4] = {0.1, 0, 0, 0.1};
KfInit(&kf, 2, KF_SENSOR_FUSION, x, p, q, r);

对于 KF_MODEL_BASED 模式，调用控制量设定方法以及预测方法，获得状态量预测值；对于 KF_SENSOR_FUSION 模式，传入第一个传感器观测状态量的数组指针，得到初步状态估计值。如：

KF_MODEL_BASEDKF_SENSOR_FUSION

C
kf.setU(&kf, u);
kf.predict(&kf);
kf.getX(&kf, x_prediction);

C
kf.predict(&kf, data1);
kf.getX(&kf, x_prediction);

传入（另一个）传感器观测状态量数组指针，调用更新方法，可获得状态量后验估计，如：

KF_MODEL_BASEDKF_SENSOR_FUSION

C
kf.update(&kf, z);
kf.getX(&kf, x_estimation);

C
kf.update(&kf, data2);
kf.getX(&kf, x_estimation);

组件说明¶

`Kf` 类¶

线性卡尔曼滤波器。

属性

名称	类型	示例值	描述
`type`	`KfType_t`	`KF_MODEL_BASED` `KF_SENSOR_FUSION`	滤波器类型
`x_dim`	`uint8_t`	`2`	状态维度
`x_x1d, P_xxd, Q_xxd, R_zzd, A_xxd, B_xxd, u_x1d, H_zxd`	`arm_matrix_instance_f32`	/	各种参数矩阵

方法

名称	参数说明	描述
`KfInit`	按声明要求传入状态维度、滤波器类型和各矩阵初值等参数	按指定的类型和给定的数据，初始化一个卡尔曼滤波器
`predict`	（仅 KF_SENSOR_FUSION 类型）传入另一组 `float*` 观测量数组地址	运行卡尔曼滤波预测部分
`update`	传入 `float*` 观测量数组地址	根据传入的观测量，运行卡尔曼滤波更新部分
`getX`	传入 `float*` 数组地址，用于存储状态量	返回当前状态量
`setU`	（仅 KF_MODEL_BASED 类型）传入 `float*` u 控制量数组地址	设置控制量
`setQ`	传入 `float*` Q 矩阵数组地址	设置过程噪声协方差 Q 矩阵
`setR`	传入 `float*` R 矩阵数组地址	设置观测噪声协方差 R 矩阵

附录¶

版本说明¶

版本号	发布日期	说明	贡献者
	2023.4.11	完成卡尔曼滤波说明文档	薛东来

参考资料¶

[1] 卡尔曼滤波器介绍

[2] DR_CAN 视频文档

此页面如何?

卡尔曼滤波器¶

理论¶

概述¶

卡尔曼增益¶

卡尔曼滤波¶

观测数据融合¶

参数调整¶

快速开始¶

使用前准备¶

示例¶

组件说明¶

Kf 类¶

附录¶

版本说明¶

参考资料¶

`Kf` 类¶