卡尔曼滤波原理与应用

最新推荐文章于 2025-03-07 09:18:30 发布

阔海星沉

最新推荐文章于 2025-03-07 09:18:30 发布

阅读量3.9k

点赞数 3

分类专栏：瑞士军刀文章标签：卡尔曼滤波算法 stm32 传感器

本文链接：https://blog.youkuaiyun.com/ManWZD/article/details/104028446

版权

瑞士军刀专栏收录该内容

19 篇文章

订阅专栏

在控制领域，获取控制对象精确的当前状态至关重要。但是在传感器测量过程中由于自身误差和外部干扰导致采样值不准确，卡尔曼滤波的作用就是为了纠正(correct)这些不准确，从而得到较准确的“状态值”(采样数据)；

文章目录

卡尔曼滤波的适用场景、原理以及使用中的关键概念

原理：
a. 卡尔曼滤波假设各种采样数据的噪音（测量值和实际值的差）符合高斯分布，并且这些采样数据之间存在某些关系；
b. 例如，有两种采样数据分别是小车的速度(v)和离障碍物的距离(d)（关系： $\Delta t*v$ ， $\space d2$ 分别是相隔一段时间的距离采样值，采样相隔时间为 $\Delta t$ ）；
c. 如下图所示，采样数据 v 和 d 的噪音分别对应高斯分布 $(\mu_0,\sigma_0)$ , $(\mu_1,\sigma_1)$ , $\mu_0$ 和 $\mu_1$ 分别代表出现概率最高的噪音值；

d. 卡尔曼滤波，参照高斯分布的乘积仍然符合高斯分布原理，根据速度(v)和离障碍物的距离(d)采样值、变化值以及关系公式，计算出一组权重公式（卡尔曼增益矩阵），对采样结果进行补偿，从而降低噪音，如上图，两个高斯分布的乘积形成的新的高斯分布，噪音的最大振幅降低了。
e. 卡尔曼滤波是一种迭代滤波器，使用不断迭代“训练”出的“规律”，对当前采样值的进行优化。
想使用卡尔曼滤波，需要具备下列条件：
a. 多种 “状态值”（不只一个），并且它们之间存在可以用公式描述的关系；
b. 这些数据都是独立传感器的采样结果；
c. 可以获得每次采样的间隔时间；
使用卡尔曼滤波中相关的概念：
a. 状态协方差矩阵 $P_k$ ：描述多“状态值”之间互相影响程度的矩阵，初始值随意，几次迭代后会逼近状态值之间的真实关系；
b. 预测矩阵 $F_k$ ：描述各种“状态值”伴随时间的变化规律，包含了各种“状态值” $S_{k-1}$ 计算下一个“状态值” $S_k$ 的计算公式，示意图如下。

c. 控制矩阵 Bk 和控制向量 μ →：描述控制指令引发“状态值”变化规律的矩阵和参数。比如，无人车内部有操控指令（加速、减速、左转、右转等），操控指令对无人车状态带来的变化时可以推断的，例如控制无人车加速，则该矩阵代表此加速度对速度的影响，为可选项。
d. 外部噪音协方矩阵 $Q_k$ ：代表可能存在的外部噪音，比如风速、地势等，为可选项。
e. 单位变换矩阵 $H_k$ ：预测值和传感器读值的单位和比例可能有区别，这个矩阵用于两个矩阵进行计算前必须的单位和比例的转换，是个常量。
f. 传感器噪音协方矩阵 $R_k$ ：代表传感器本身的误差噪音， $R_k$ 的中心值$ \overrightarrow{z} $等于传感器本次的测量值， $R_k$ 由传感器厂家提供。
g. 卡尔曼增益矩阵 $K$ : 卡尔曼滤波的核心，示意图如下，本质上是一个融合预测值和测量值的一个权重矩阵，融合的过程就是用“预测值”去纠正“测量值”的过程。

h. 迭代更新的计算流程图

实践场景

霍尔传感器测速
a. 硬件配置：玩具车轮上安装 3 个磁铁，搭配一个霍尔传感器
b. 基本原理：使用 STM32 定时器获得进出磁场的时间戳
c. 定时器累加频率，20KHz
超声波传感器测距
测距实验数据：接近障碍物时采样值偏大，会引起避障不及时

实际距离 cm	测量距离 cm
12	21
20	28
41.5	49
60	65
121.5	127
161.5	149-152
201.5	186-188
281.5	232-234
361.5	310-327

为什么需要卡尔曼滤波：超声波传感器受环境影响太大，尤其是接近障碍物时采样值偏大，希望通过滤波纠正。

套用卡尔曼滤波公式

1、小车状态描述：
$\begin{bmatrix} v \\ d \end{bmatrix}$

x : 为小车当前状态

v : 当前方向上的行驶速度，单位为 cm/s

d : 当前行进方向上离障碍物的距离，单位为 cm

2、预测矩阵 F ，使用恒定速度模型:
$d_{k+1} = d_k - \Delta t \times v$
$x_{k+1} = F * x_k$
$\begin{bmatrix} 1 & 0 \\ -\Delta t/1000 & 1 \end{bmatrix}$
$\Delta t$ 为每次迭代计算的间隔时间，单位为 ms

3、测量矩阵 H ，传感器测量出的既是速度和距离

H = $\begin{bmatrix} 1 & 0 \\ 0 & 1 \end{bmatrix}$

4、运动噪音协方差矩阵 Q，也就是可能随机存在的小车加/减速的过程（随机的加速度 $a$ 造成的），定义为正态分布 $N (0, Q)$

速度噪音： $\Delta v = a \times \Delta t$ ，符合平均值为 0 的正态分布

距离噪音： $\Delta d = -\frac{1}{2} \times a \times \Delta t^2$ ，符合平均值为 0 的正态分布

注意： $\Delta v \space \Delta d$ 线是性相关的( $\rho_{\Delta v \Delta d} = 1$ ，即相关系数为 1)
$\sigma_{\Delta v}^2 = cor(\Delta v) = cor(a \times \Delta t) = Delta t^2 \times \sigma_a^2$
$\sigma_{\Delta d}^2 = cor(\Delta d) = cor(\frac{1}{2} \times a \times \Delta t^2) = \frac{1}{4} \times \Delta t^4 \times \sigma_a^2$
$\sigma_{\Delta v \Delta d} = cor(\Delta v,\Delta d) = \rho_{\Delta v \Delta d} \times \sqrt{\sigma_{\Delta v}^2} \times \sqrt{\sigma_{\Delta d}^2} = \frac{1}{2} \times \Delta t^3 \times \sigma_a^2$

$\begin{bmatrix} \sigma_{\Delta v}^2 & \sigma_{\Delta v \Delta d} \\ \sigma_{\Delta d \Delta v} & \sigma_{\Delta d}^2 \end{bmatrix}\ = \begin{bmatrix} (\Delta t/1000)^2 & \frac{1}{2}(\Delta t/1000)^3 \\ \frac{1}{2}(\Delta t/1000)^3 &\frac{1}{4}(\Delta t/1000)^4 \end{bmatrix} * \sigma_a^2$

其中 a = 50cm/s2 ， $\sigma_a^2$ 设定为 625 $cm/s^2)^2$ ，即 $2\sigma_a=50$ !，加速度在 50 $cm/s^2$ 以内的概率在 60% 以上。

5、控制矩阵 $B$ 和控制向量 $\overrightarrow{\mu}$

a) 小车的加减速控制方式很简陋并且非常灵活

b) 涉及到很多随机因素（电池电量、是否在转弯、速度变化的梯度大小）

c) 定义为控制矩阵的话会非常复杂

d) 因此把控制导致的速度变化定义为了“随机”噪音，由运动噪音矩阵 $Q$ 表示。

6、测量噪音 $R$ ，也就是传感器的精度，定义为正态分布 N($ \overrightarrow{z}$, $R$ )

$\overrightarrow{z}$ : 为每次传感器的测量值

测速误差 : 最大误差为 5% ， $2\sigma_{ve}=0.05v$

测距误差 : 最大误差为 10% ， $2\sigma_{vd}=0.1d$

注意：因为速度和距离的测量是完全独立的两个传感器，相关系数为 0，因此两者误差的协方差为 0

$\sigma_{ve}^2=0.000625 \times v^2$

$\sigma_{de}^2=0.0025 \times d^2$

$c o r (v e, d e) = 0$

$\begin{bmatrix} 0.000625 & 0 \\ 0 & 0.0025 \end{bmatrix}$

$\begin{bmatrix} 0.000625 \times v^2 & 0 \\ 0 & 0.0025 \times d^2 \end{bmatrix} = diag(diag(E* \overrightarrow{z} )* \overrightarrow{z} )$

7、协方矩阵 $P$ 初始化， $P$ 是一个迭代更新的量，每一轮预测和更新后， $P$ 都会更新一个新的值，因此初始化时可以根据估计协定，因为随着几轮迭代会越来越趋近真实值。

$\begin{bmatrix} 1 & 0 \\ 0 & 1 \end{bmatrix}$

8、套用公式后结果为：

$\hat{x}_k = F_k\hat{x}_{k-1}$

$P_k = F_kP_{k-1}F_k^T + Q_k$

$\hat{x}_k' = \hat{x}_k + K'(\overrightarrow{z_k} - \hat{x}_k)$

$P_k' = P_k - K'P_k$

$\frac{P_k}{P_k + R_k}$

利用 matlab 把二维矩阵运算转换为符号运算

由于 STM32 单片机中存储空间有限，计算能力有限，调用矩阵计算函数库不现实，所以可以利用 matlab 的符号定义功能，把矩阵运算转换为符号运算。定义迭代更新中使用的变量：p11 p12 p21 p22 ve de vk dk delt_t cor_a cor_ve cor_de ，它们的含义如下：

运动噪音的加速度方差 $\sigma_a^2= cor\_a$

传感器误差矩阵 $\begin{bmatrix} cor\_v &0 \\ 0 & cor\_de \end{bmatrix}$

本次迭代初始协方矩阵 $P_{k-1} = \begin{bmatrix} p11 & p12 \\ p21 & p22 \end{bmatrix}$

本次迭代初始状态 $\hat{x}_{k-1}=\begin{bmatrix} ve \\ de \end{bmatrix}$

本次测量值 $\overrightarrow{z_k} = \begin{bmatrix} vk \\ dk \end{bmatrix}$

本次测量时间间隔 $\Delta t_k = delta\_t$

% 把二维的卡尔曼滤波的矩阵运算转换为普通运算
clear; close all; clc
% 符号定义
syms p11 p12 p21 p22 vk dk ve de delt_t;
% 初始状态
P_km1 = [p11, p12; p21, p22];
x_km1 = [ve; de];
% 预测矩阵
F = [1, 0; -delt_t/1000, 1];
Ft = [1, -delt_t/1000; 0, 1];
% 运动噪音协方差矩阵
cor_a = 10000;
Q = [(delt_t/1000)^2, 1/2*(delt_t/1000)^3; 1/2*(delt_t/1000)^3, 1/4*;(delt_t/1000)^4]*cor_a;
% 预测状态
x_exp_k = F*x_km1;
P_k = F*P_km1*Ft + Q;
% 测量状态
z_k = [vk; dk];
% 测量噪音
cor_ve = 0.000625;
cor_de = 0.0025;
R_k = [cor_ve*vk^2, 0; 0, (150/dk)^2];
%卡尔曼增益
K = P_k/(P_k + R_k);
%更新状态
P_k_target = P_k - K*P_k;
x_exp_k_target = x_exp_k + K*(z_k - x_exp_k);