失眠网 > 控制算法学习一卡尔曼滤波（1）以小车为例

控制算法学习一卡尔曼滤波（1）以小车为例

时间：2024-06-01 12:30:29

控制算法学习一、卡尔曼滤波（1）以小车为例

前言线性系统状态方程和观测方程举例卡尔曼滤波状态预测状态更新卡尔曼滤波流程图

前言

卡尔曼滤波（Kalman Filter, KF）是最经典的传感器滤波/去噪/估计/预测算法，适用于线性系统的最优状态估计，并且衍生出扩展卡尔曼滤波（EKF）、迭代卡尔曼滤波（IKF），无迹卡尔曼滤波（UKF）和容积卡尔曼滤波（CKF）等。

本文将非常简单的介绍和推导卡尔曼滤波，加深KF的理解记忆。

线性系统

什么是线性系统？控制领域中，系统输入输出满足叠加原理（叠加性和齐次性）就是线性系统。

一个简单的例子，一维路径上运行的匀速小车，时间（输入）和位置（输出）就是线性的。

状态方程和观测方程

如果只知道小车的初始速度，怎么获得小车在任一时刻的位置呢？

第一种方法是直接通过匀速计算： p = v ∗ t p=v*t p=v∗t，但是现实中路面可能会有崎岖，导致结果出现偏差；

第二种方法是在小车上加传感器（比如GPS），但GPS也会有测量误差；

因此以上两种方法就分别可以用状态方程和观测方程来表示：

x k = A x k − 1 + B u k − 1 + w k − 1 , w k ∼ [ 0 , Q ] z k = H x k + v k , v k ∼ [ 0 , R ] \begin{aligned} & x_k = Ax_{k-1} + Bu_{k-1} + w_{k-1}, w_k\sim [0, Q] \\ & z_k = Hx_k+v_{k} ,v_k\sim[0, R] \end{aligned} xk=Axk−1+Buk−1+wk−1,wk∼[0,Q]zk=Hxk+vk,vk∼[0,R]

A , B , H A,B,H A,B,H分别表示状态转移矩阵，控制（输入）转换矩阵，观测转换矩阵。

w k , v k w_k,v_k wk,vk都是高斯白噪声，表示了系统误差（过程噪声）和测量误差。

x k , z k x_k,z_k xk,zk是向量，表明k时刻的系统状态。

举例

对于匀速运动的小车而言，可以这样建模：

状态方程

[ p v ] t = [ 1 Δ t 0 1 ] [ p v ] t − 1 + w k − 1 \left [ \begin{matrix} p \\ v \\ \end{matrix} \right ]_{t}= \left [ \begin{matrix} 1 & \Delta t \\ 0 & 1 \end{matrix} \right ] \left [ \begin{matrix} p \\ v \end{matrix} \right ]_{t-1} \ + w_{k-1} [pv]t=[10Δt1][pv]t−1+wk−1

观测方程（z是GPS读数）

[ z ] t = [ 1 ] [ p ] t + v k \left [ \begin{matrix} z \\ \end{matrix} \right ]_{t}= \left [ \begin{matrix} 1 \\ \end{matrix} \right ] \left [ \begin{matrix} p \end{matrix} \right ]_{t} \ + v_{k} [z]t=[1][p]t+vk

卡尔曼滤波

KF有两个流程，先预测，再更新，其实就是把预测得到的状态与观测得到的状态，以某种方式进行加权更新，得到状态的最优估计。

状态预测

预测过程有两个公式：

x ~ k = A x k − 1 + B u k − 1 ( 1 ) P ~ k = A P k − 1 A T + Q ( 2 ) x k = [ p v ] P k = [ σ p p σ p v σ v p σ v v ] \begin{aligned} & \tilde x_k = Ax_{k-1}+Bu_{k-1} &\quad (1)\\ & \tilde P_k=AP_{k-1}A^T+Q &\quad (2)\\ & x_k= \left [ \begin{matrix} p \\ v \\ \end{matrix} \right ] \quad P_{k}= \left [ \begin{matrix} \sigma_{pp} & \sigma_{pv} \\ \sigma_{vp} & \sigma_{vv} \\ \end{matrix} \right ] \end{aligned} x~k=Axk−1+Buk−1P~k=APk−1AT+Qxk=[pv]Pk=[σppσvpσpvσvv](1)(2)

这里增加了一个状态协方差矩阵 P P P（对称矩阵），用来描述状态量之间的关系。

这两个公式描述了：

(1) 将k-1时刻的状态 x k − 1 x_{k-1} xk−1，通过状态转移矩阵和控制转换矩阵，预测k时刻的状态 x k x_k xk;

(2) 将k-1时刻的状态协方差矩阵 P k − 1 P_{k-1} Pk−1，通过状态转移矩阵和过程噪声，预测k时刻的状态协方差矩阵 P k − 1 P_{k-1} Pk−1；

公式（1）容易理解，公式（2）需要从随机过程上解释：

X = [ x 1 , x 2 , … , x n ] T , Y = [ y 1 , y 2 , … , y n ] T X=[x_1,x_2,\dots,x_n]^T, Y=[y_1,y_2,\dots,y_n]^T X=[x1,x2,…,xn]T,Y=[y1,y2,…,yn]T， X , Y X,Y X,Y独立，则有 P ( A X + Y ) = A P X A T + P Y P(AX+Y)=AP_XA^T+P_Y P(AX+Y)=APXAT+PY

还是用小车作为例子，公式（1）其实就是在用匀速运动模型，通过上一时刻小车的位置预测当前小车的位置。公式（2）则是通过上一时刻小车的速度和位置关系，预测当前速度和位置的关系，并且叠加过程噪声带来的不确定性 Q Q Q

状态更新

更新过程有三个公式：

K k = P ~ k H T ( H P k H T + R ) − 1 x k = x ~ k + K k ( z k − H x ~ k ) P k = ( E − K k H ) P ~ k \begin{aligned} & K_k = \tilde P_kH^T(HP_kH^T+R)^{-1} \\ & x_k = \tilde x_k + K_k(z_k - H \tilde x_k) \\ & P_k = (E-K_kH)\tilde P_k \end{aligned} Kk=P~kHT(HPkHT+R)−1xk=x~k+Kk(zk−Hx~k)Pk=(E−KkH)P~k

这里增加了卡尔曼增益 K k K_k Kk，实际上就是预测和观测权重。

这三个公式描述了：

（1）当前卡尔曼增益的计算；

（2）通过卡尔曼增益，进行预测\观测值的加权；

（3）通过卡尔曼增益，更新协方差矩阵；

本质上，以上三个公式是对预测状态和观测状态这两个高斯分布进行相乘，获得的高斯分布，就是最优状态估计。所谓的卡尔曼增益，就是预测状态的高斯分布与融合后的高斯分布的不确定性（协方差矩阵）的比值。