前沿

在智能控制、SLAM、机器人、时间序列等领域，我们经常会涉及到一个算法叫做卡尔曼滤波算法 Kalman Filter
这是一个牛逼到“上了天”的算法，这个滤波器是由Swerling (1958), Kalman (1960)与 Kalman and Bucy (1961)在论文中发表的，后来阿波罗飞船上天的时候，导航就是用了这个算法。
总的来说，卡尔曼滤波是一个最后化的算法，通过不断的迭代，求出自身状态的最优估计。下面，我们用一个机器人的栗子说明卡尔曼滤波算法是什么。

栗子

如果你家地下室有一个黑漆漆的小洞，这个小洞深不见底，但是你可以确定的是，这个小洞是笔直的，想要探寻小洞里面有什么的你制作了一个小车，这个小车也只能前进后退，毕竟洞的宽度也不能够左右移动，你还有一个超声波测距装置，能够测出小车距离洞口有多远，但是这个测距装置有时候也没有那么准，总是会存在误差，这时候你想知道你通过遥控控制的小车到底距离你多少远，你就可以用到卡尔曼滤波了。
我们设小车ttt时刻的状态为xtx_txt，这个xk=(pt,vt)x_k=(p_t,v_t)xk=(pt,vt)，ptp_tpt代表的是ttt时刻小车的距离，vtv_tvt代表的是ttt时刻小车的速度，我们通过简单建模（卡尔曼滤波需要建立状态之间的预测模型即xt−1x_{t-1}xt−1到xtx_txt的关系，在KF中这种关系一般是线性关系，所以KF用在线性模型，EKF扩展卡尔曼滤波可以用在非线性模型）得到：
pt=pt−1+vt−1⋅Δt+Ut⋅Δt22(1)p_t=p_{t-1}+v_{t-1}·\Delta t + U_t ·\frac{\Delta t^2}{2}\ \ \ \ \ \ (1)pt=pt−1+vt−1⋅Δt+Ut⋅2Δt2      (1)
vt=vt−1+ut⋅Δt(2)v_t=v_{t-1}+u_{t}·\Delta t \ \ \ \ \ \ (2)vt=vt−1+ut⋅Δt      (2)
此处的UtU_{t}Ut是遥控器控制电机造成的加速度，这个方程在初中就教过，(1)表示的是小车下一时刻的位置等于上一时刻位置加上时间间隔中相差的距离，(2)表示下一秒速度的上一秒速度和上一秒加速度在间隔时间中改变的速度增量。因此我们这个小车的线性预测方程就可以用下面这种形势表示出来
[ptvt]=[1Δt01][pt−1vt−1]+[Δt22Δt]ut(3)\begin{bmatrix} p_t \\ v_t\end{bmatrix}=\begin{bmatrix}1&\Delta t \\0&1 \end{bmatrix}\begin{bmatrix}p_{t-1}\\v_{t-1}\end{bmatrix}+\begin{bmatrix}\frac{\Delta t^2}{2}\\ \Delta t \end{bmatrix}u_t\ \ \ \ \ \ (3)[ptvt]=[10Δt1][pt−1vt−1]+[2Δt2Δt]ut      (3)
Ft:=[1Δt01]和Bt:=[Δt22Δt]是状态变化矩阵Ft:=\begin{bmatrix}1&\Delta t \\0&1 \end{bmatrix}\text{和}B_t:=\begin{bmatrix}\frac{\Delta t^2}{2}\\ \Delta t \end{bmatrix}\text{是状态变化矩阵}Ft:=[10Δt1]和Bt:=[2Δt2Δt]是状态变化矩阵
所以上式也可以写成
x^t−=Ftxt−1^+Btut(4)\hat x_t ^-=F_t\hat{x_{t-1}}+B_tu_t\ \ \ \ \ \ \ (4)x^t−=Ftxt−1^+Btut       (4)
x^t−\hat x_t ^-x^t−中 ^\hat{}^ 表示这是一个估计值，而不是真实值，−^-−表示这并非最佳的估计（因为这个预测方程是理想状况，现实中有很多不可预料的噪声，比如路面的不平坦（最佳估计在非最佳估计x和传感器数据滤波融合之后得到，式子中的xt−1^\hat{x_{t-1}}xt−1^））
（4）就是卡尔曼滤波器的第一个公式——状态预测公式，FtFtFt表示我们如何从上一个状态推出下一个状态（不同的场景状态预测公式细节表达都不同，此处是线性系统。
卡尔曼滤波的第二个公式和状态的方差有关，因为噪声的存在，状态是一个随机变量，卡尔曼滤波假设状态服从正态分布，所以我们用PtP_tPt表示在t时刻状态的方差，在这个问题中
Pt=[ΣppΣpvΣvpΣvv]P_t=\begin{bmatrix}\Sigma_{pp}&\Sigma_{pv}\\ \Sigma_{vp} & \Sigma_{vv}\end{bmatrix}Pt=[ΣppΣvpΣpvΣvv]
那么在经过线性的状态转移之后，我们的均值（也就是x^t−\hat x_t ^-x^t−）的变化如（4）所示，方差PtP_tPt的变化是如何呢？
通过下面这个公式
cov(x)=Σcov(x)=\Sigmacov(x)=Σcov(Ax)=AΣATcov(Ax)=A \Sigma A^Tcov(Ax)=AΣAT
得到Pt=FtPt−1FtT+Qt(5)P_t=F_tP_{t-1}F_t^T+Q_t \ \ \ \ \ \ \ \ \ \ \ (5)Pt=FtPt−1FtT+Qt           (5)
这里的QtQ_tQt表示外部环境的不确定性，造成的方差
（5）就是卡尔曼滤波器的第二个公式，状态方差公式
关于预测状态方程的部分已经结束了，我们现实生活中，电子设备一般有好几个传感器来提高其稳定性，我们的小车现在就有一个超声波传感器，因为洞里深不见底，所以假设我们拿着一个隔板放在洞口，让小车测量到到洞口的距离，这个数据也充满着噪声，车的抖动或者其他原因，这个数据也有一个均值ztz_tzt和方差RtR_tRt
zt=Hkxkz_t=H_k x_kzt=HkxkRt=HkPkHkTR_t=H_kP_kH_k^TRt=HkPkHkT这里的HkH_kHk是传感器的观测矩阵，表明如何通过某种状态得到当前的传感器观测值。
那么当两个正态分布的参数了解时，如同下图，我们如何得到我们与其最优的估计值呢？一个很简单的想法，就是将这两个正泰分布相乘，变成一个正态分布。
我们知道一维的正态分布如下：
N(x,μ,σ)=1σ2πe−(x−μ)22σ2N(x,\mu ,\sigma)=\frac{1}{\sigma \sqrt{2\pi}}e^{-\frac{(x-\mu)^2}{2\sigma^2}}N(x,μ,σ)=σ2π1e−2σ2(x−μ)2
那么设预测方程得到的正态分布为N(x,μ0,σ0)N(x,\mu_0 ,\sigma_0)N(x,μ0,σ0)，传感器观测得到的正态分布为N(x,μ1,σ1)N(x,\mu_1 ,\sigma_1)N(x,μ1,σ1)
求解N(x,μ0,σ0)N(x,μ1,σ1)=N(x,μ′,σ′)N(x,\mu_0 ,\sigma_0 )N(x,\mu_1 ,\sigma_1)=N(x,\mu' ,\sigma')N(x,μ0,σ0)N(x,μ1,σ1)=N(x,μ′,σ′)
可以得到（我手算了以下，下面结论是对的，但是正推我推不出来， = =太菜了）
μ′=μ0+σ02(μ1−μ0)σ02+σ12\mu'=\mu_0 +\frac{\sigma _0^2(\mu_1-\mu_0)}{\sigma _0^2+\sigma _1^2} μ′=μ0+σ02+σ12σ02(μ1−μ0)
σ′2=σ02−σ04σ02+σ12\sigma'^2=\sigma_0^2-\frac{\sigma^4_0}{\sigma_0^2+\sigma_1^2}σ′2=σ02−σ02+σ12σ04
如果是多纬空间，我们可以把这个式子转化成矩阵格式（也就是卡尔曼滤波器的剩下三个方程）
K=Σ0(Σ0+Σ1)−1K=\Sigma_0(\Sigma_0+\Sigma_1)^{-1}K=Σ0(Σ0+Σ1)−1
μ⃗′=μ⃗0+K(μ⃗1−μ⃗0)\vec\mu'=\vec\mu_0 +K(\vec\mu_1-\vec\mu_0) μ′=μ0+K(μ1−μ0)
Σ′=Σ0−KΣ0\Sigma'=\Sigma_0-K\Sigma_0Σ′=Σ0−KΣ0
所以卡尔曼滤波由以下五个方程得到
x^t−=Ftxt−1^+Btut\hat x_t ^-=F_t\hat{x_{t-1}}+B_tu_tx^t−=Ftxt−1^+Btut
Pt=FtPt−1FtT+QtP_t=F_tP_{t-1}F_t^T+Q_t Pt=FtPt−1FtT+Qt
K=Σ0(Σ0+Σ1)−1K=\Sigma_0(\Sigma_0+\Sigma_1)^{-1}K=Σ0(Σ0+Σ1)−1
μ⃗′=μ⃗0+K(μ⃗1−μ⃗0)\vec\mu'=\vec\mu_0 +K(\vec\mu_1-\vec\mu_0) μ′=μ0+K(μ1−μ0)
Σ′=Σ0−KΣ0\Sigma'=\Sigma_0-K\Sigma_0Σ′=Σ0−KΣ0

EKF

而所谓的扩展卡尔曼滤波，就是在非线性环境（非线性状态预测方程，非线性观测方程）的情况下，对这些非线性方程进行一阶泰勒展开后，近似成线性，用KF来解决。

总结

卡尔曼滤波是线性系统正态分布的噪声下的最佳估计，但是需要注意的是我们需要知道
1.这个系统的状态转移方程
2.里面传感器的观测矩阵（也就是如何计算出状态的方法）
3.测量出观测数据噪声的方差。

CODE

附：python代码:预测小车位置和速度的一段程序，观测的值是一段从0到99，速度为1的匀速行驶路径，加入的噪声是均值为0，方差为1的高斯噪声。¹

import numpy as np
import matplotlib.pyplot as plt# 创建一个0-99的一维矩阵
z = [i for i in range(100)]
z_watch = np.mat(z)
#print(z_mat)# 创建一个方差为1的高斯噪声，精确到小数点后两位
noise = np.round(np.random.normal(0, 1, 100), 2)
noise_mat = np.mat(noise)# 将z的观测值和噪声相加
z_mat = z_watch + noise_mat
#print(z_watch)
# 定义x的初始状态
x_mat = np.mat([[0,], [0,]])
# 定义初始状态协方差矩阵
p_mat = np.mat([[1, 0], [0, 1]])
# 定义状态转移矩阵，因为每秒钟采一次样，所以delta_t = 1
f_mat = np.mat([[1, 1], [0, 1]])
# 定义状态转移协方差矩阵，这里我们把协方差设置的很小，因为觉得状态转移矩阵准确度高
q_mat = np.mat([[0.0001, 0], [0, 0.0001]])
# 定义观测矩阵
h_mat = np.mat([1, 0])
# 定义观测噪声协方差
r_mat = np.mat([1])
for i in range(100):x_predict = f_mat * x_mat#根据初始状态和运动方程估计现在的状态p_predict = f_mat * p_mat * f_mat.T + q_mat#计算估计的方差kalman = p_predict * h_mat.T / (h_mat * p_predict * h_mat.T + r_mat)#计算卡尔曼增益x_mat = x_predict + kalman *(z_mat[0, i] - h_mat * x_predict)#信息融合print(x_mat)p_mat = (np.eye(2) - kalman * h_mat) * p_predictplt.plot(x_mat[0, 0], x_mat[1, 0], 'ro', markersize = 3)#横坐标是路程p 纵坐标是速度v
plt.show()

参考
https://zhuanlan.zhihu.com/p/39912633
https://blog.csdn.net/codesamer/article/details/81191487
https://zhuanlan.zhihu.com/p/85865058
https://zhuanlan.zhihu.com/p/67138271

来自https://blog.csdn.net/codesamer/article/details/81191487 ↩︎

KalmanFilter相关推荐

[Tracking] KCF + KalmanFilter目标跟踪
基于KCF和MobileNet V2以及KalmanFilter的摄像头监测系统简介这是一次作业.Tracking这一块落后Detection很多年了,一般认为Detection做好了,那么只要能 ...
实战 | keras-yolov3 + Kalman-Filter 进行人体多目标追踪（含代码）
点击上方"小白学视觉",选择加"星标"或"置顶" 重磅干货,第一时间送达本文转自|计算机视觉联盟今天给大家分享一个非常棒非常炫酷的gith ...
Opencv cv2.KalmanFilter 鼠标跟踪
cv2.KalmanFilter 实现鼠标跟踪 import cv2 import numpy as npdef mousemove(event, x, y, s, p):#global frame, ...
学习OpenCV2——卡尔曼滤波(KalmanFilter)详解
本文将简要回顾一下卡尔曼滤波理论,然后详细介绍如何在OpenCV中使用卡尔曼滤波进行跟踪,最后给两个程序实例. 1. 卡尔曼滤波理论回顾对于一个动态系统,我们首先定义一组状态空间方程状态方程: ...
OpenCV--卡尔曼滤波(KalmanFilter)详解【转载】
本文将简要回顾一下卡尔曼滤波理论,然后详细介绍如何在OpenCV中使用卡尔曼滤波进行跟踪,最后给两个程序实例. 1. 卡尔曼滤波理论回顾对于一个动态系统,我们首先定义一组状态空间方程状态方程: 测 ...
keras-yolov3 + Kalman-Filter 进行人体多目标追踪（含代码）
笔者最近在做新零售智慧门店的相关项目,主要涵盖人流量.人物活动区域轨迹等.那么本篇其实是笔者在实践过程中一个"失败"的案例,因为其应用复用在现实场景的时候效果非常差,所以只是当做练 ...
1.1.2.卡尔曼滤波-KalmanFilter
更多内容,请关注: github:Autopilot-Updating-Notes gitee: Autopilot-Updating-Notes 1.1.2.1 简介对于这个滤波器,我们几乎可以下 ...
qchart折现图_Qt Charts 动态实时折线图绘制
在Qt Charts发布之前, Qt比较著名两个画图插件是 qwt和Qcustom, 其中Qcustom较轻量,只需要在project 中包含qcustomplot.h 和 qcustomplot.c ...
OpenCV卡尔曼滤波介绍与代码演示
点击上方"小白学视觉",选择加"星标"或"置顶" 重磅干货,第一时间送达卡尔曼滤波原理卡尔曼滤波最早可以追溯到Wiener滤波,不同的是 ...

KalmanFilter

前沿

栗子

EKF

总结

CODE

KalmanFilter相关推荐

最新文章

热门文章