1344 字
7 分钟
[RM] 卡尔曼滤波快速入门
2025-09-02
RM
RM
/
卡尔曼滤波

本文旨在以最快的速度学会卡尔曼滤波的使用方法，并不做理论推导和进一步的深入理解

诶我草了我一个学计算机的为什么要学现代控制理论的知识啊

五条最重要的公式#

状态预测 x^k∣k−1=Fkx^k−1∣k−1+Bkuk协方差预测 Pk∣k−1=FkPk−1∣k−1FkT+Qk卡尔曼增益计算 Kk=Pk∣k−1HkT(HkPk∣k−1HkT+Rk)−1状态更新 x^k∣k=x^k∣k−1+Kk(zk−Hkx^k∣k−1)协方差更新 Pk∣k=(I−KkHk)Pk∣k−1\begin{aligned} \text{状态预测 } \hat{x}_{k|k-1} &= F_k \hat{x}_{k-1|k-1} + B_k u_k \\ \text{协方差预测 } P_{k|k-1} &= F_k P_{k-1|k-1} F_k^T + Q_k \\ \text{卡尔曼增益计算 } K_k &= P_{k|k-1} H_k^T (H_k P_{k|k-1} H_k^T + R_k)^{-1} \\ \text{状态更新 } \hat{x}_{k|k} &= \hat{x}_{k|k-1} + K_k (z_k - H_k \hat{x}_{k|k-1}) \\ \text{协方差更新 } P_{k|k} &= (I - K_k H_k) P_{k|k-1} \end{aligned}

什么是x^\hat x状态？#

x^\hat x表示对系统状态的估计值，通常是一个向量，包含了系统的各种状态变量，比如位置、速度、加速度等

比如一个物体正在直线运动，它当前的真实状态就可以表示为

x=[positionvelocityacceleration]x = \begin{bmatrix} position \\ velocity \\ acceleration \end{bmatrix}

x^\hat x和xx有什么区别？#

xx表示真实状态，而x^\hat x表示状态的估计值

NOTE

真实状态xx是我们不知道的，而卡尔曼滤波的目的就是通过测量值和估计值x^k∣k−1\hat x_{k|k-1}计算出x^k∣k\hat x_{k|k}来尽量接近真实状态xx

x^k∣k−1\hat x_{k|k-1}是什么意思？#

x^k∣k−1\hat x_{k|k-1}表示在当前时刻kk，基于时刻k−1k-1的信息进行的对当前时刻kk预测，但并未融合kk时刻的测量值zkz_k，也称为先验估计/先验状态

x^k∣k\hat x_{k|k}又是什么意思？#

x^k∣k\hat x_{k|k}是在当前时刻kk，由x^k∣k−1\hat x_{k|k-1}和kk时刻的测量值zkz_k计算得出的更接近kk时刻的真实状态的估计值，也称为后验估计/后验状态

这个后验状态x^k∣k\hat x_{k|k}就是我们最终想要的结果

什么是观测矩阵HkH_k？#

观测矩阵HkH_k定义了如何从状态变量x^\hat x映射到测量值zkz_k

TIP

举个例子，假设我们使用的测量仪器只能测量物体的位置，不能测量速度和加速度

所以HkH_k就应该是

Hk=[100]H_k = \begin{bmatrix} 1 & 0 & 0 \end{bmatrix}

因为

z=Hkx=[100][positionvelocityacceleration]=[position]z = H_k x = \begin{bmatrix} 1 & 0 & 0 \end{bmatrix} \begin{bmatrix} position \\ velocity \\ acceleration \end{bmatrix} = \begin{bmatrix} position \end{bmatrix}

什么是状态转移矩阵FkF_k？#

状态转移矩阵FkF_k定义了系统状态如何从时刻k−1k-1转移到时刻kk

举个例子，一个物体在匀加速直线运动，假设每次时间步长为Δt\Delta t，那么状态转移矩阵FkF_k可以表示为

Fk=[1Δt12Δt201Δt001]F_k = \begin{bmatrix} 1 & \Delta t & \frac{1}{2} \Delta t^2 \\ 0 & 1 & \Delta t \\ 0 & 0 & 1 \end{bmatrix}

因为

xk=Fkxk−1=[1Δt12Δt201Δt001][positionvelocityacceleration]k−1=[positionvelocityacceleration]kx_k = F_k x_{k-1} = \begin{bmatrix} 1 & \Delta t & \frac{1}{2} \Delta t^2 \\ 0 & 1 & \Delta t \\ 0 & 0 & 1 \end{bmatrix} \begin{bmatrix} position \\ velocity \\ acceleration \end{bmatrix}_{k-1} = \begin{bmatrix} position \\ velocity \\ acceleration \end{bmatrix}_k

换成高中物理的写法就是

pk=pk−1+vk−1Δt+12ak−1Δt2vk=vk−1+ak−1Δtak=ak−1\begin{aligned} p_k &= p_{k-1} + v_{k-1} \Delta t + \frac{1}{2} a_{k-1} \Delta t^2 \\ v_k &= v_{k-1} + a_{k-1} \Delta t \\ a_k &= a_{k-1} \end{aligned}

这下看懂了

什么是控制输入矩阵BkB_k和uku_k？#

控制输入矩阵BkB_k定义了外部控制输入uku_k如何影响系统状态

TIP

简单来说就像你在匀速驾驶一辆车，突然猛踩了一下油门，车的加速度会增加，从而影响车的速度和位置

而控制输入矩阵BkB_k就是用来描述这种外部控制（猛踩一下油门）对系统状态（车的加速度）的影响

而uku_k就是这个外部控制输入的具体数值，也就是kk时刻踩油门踩了多深

举个例子，假设我们在kk时刻对直线运动的物体施加一个额外的力Δforce\Delta force作为控制输入uku_k

uk=Δforceu_k = \Delta force

那么根据牛顿第二定律F=maF=ma就能得到加速度的改变量

Δa=Δforcem\Delta a = \frac{\Delta force}{m}

因为卡尔曼滤波是个离散系统，所以我们不关心加速度的连续变化，只关心在每个时间步长Δt\Delta t内加速度的变化量Δa\Delta a

控制输入矩阵BkB_k就可以表示为

Bk=[12Δt21mΔt1m1m]B_k = \begin{bmatrix} \frac{1}{2} \Delta t^2 \frac{1}{m} \\ \Delta t \frac{1}{m}\\ \frac{1}{m} \end{bmatrix}Bkuk=[12Δt21mΔt1m1m]Δforce=[ΔpositonΔvelocityΔacceleration]B_k u_k = \begin{bmatrix} \frac{1}{2} \Delta t^2 \frac{1}{m} \\ \Delta t \frac{1}{m}\\ \frac{1}{m} \end{bmatrix} \Delta force = \begin{bmatrix} \Delta positon \\ \Delta velocity \\ \Delta acceleration \end{bmatrix}

什么是过程噪声协方差矩阵QkQ_k？#

过程噪声协方差矩阵QkQ_k表示系统在状态转移过程中可能存在的随机扰动或不确定性

比如风吹、路面不平等因素都会影响物体的运动状态，这个在kk时刻对物体的不确定影响我们就用QkQ_k来表示

什么是测量噪声协方差矩阵RkR_k？#

测量噪声协方差矩阵RkR_k表示测量仪器本身存在的误差或不确定性

比如测量仪器的精度有限，可能会有一定的误差，这个误差我们就用RkR_k来表示

什么是协方差矩阵PP？#

主包也不知道，等主包学完概率论了再来补充

更适合程序中使用的协方差更新公式#

由于浮点数运算是有误差的，直接使用

Pk∣k=(I−KkHk)Pk∣k−1P_{k|k} = (I - K_k H_k) P_{k|k-1}

可能会导致协方差矩阵PP变得不正定

所以更推荐使用下面的公式来更新协方差矩阵PP

Pk∣k=(I−KkHk)Pk∣k−1(I−KkHk)T+KkRkKkTP_{k|k} = (I - K_k H_k) P_{k|k-1} (I - K_k H_k)^T + K_k R_k K_k^T
[RM] 卡尔曼滤波快速入门
https://a1kari8.github.io/posts/kalman_filter/
作者
A1kari8
发布于
2025-09-02
许可协议
CC BY-NC-SA 4.0
文章加密测试
[工科复变函数] 复数基础
© 2026 A1kari8. All Rights Reserved. / RSS / Sitemap
Powered by Astro & Fuwari
Inspired by Mizuki