什么是卡尔曼滤波
对于这个滤波器,我们几乎可以下这么一个定论:只要是存在不确定信息的动态系统,卡尔曼滤波就可以对系统下一步要做什么做出有根据的推测。即便有噪声信息干扰,卡尔曼滤波通常也能很好的弄清楚究竟发生了什么,找出现象间不易察觉的相关性。
因此卡尔曼滤波非常适合不断变化的系统,它的优点还有内存占用较小(只需保留前一个状态)、速度快,是实时问题和嵌入式系统的理想选择。
形象解释
卡尔曼滤波就是以上个时刻的状态,推断下个时刻的状态。
比如一个有经验的船长,没有很先进的仪器能定位船的位置,他需要走一会判断一下位置,然后根据这个位置判断下一个位置,否则走的太远就迷路了;
首先他能知道起点的位置,然后每隔一定时间,就根据船速、航线、风向等预估自己的位置,这是经验,不一定准确,且肯定存在一定误差;
他还有个不太精确的仪器,能测量出自己的位置,也不一定准确,且也存在一定误差;
那现在有了经验值和测量值,到底哪个准确,如何参考这两个值;
如果船长很自信,更相信自己的经验,这样 经验值的权重就大一些,观测值权重小一些;
如果船长经验不足,肯定不能过分相信经验,只能更多参考测量值,此时测量值权重大一些;
最后就确定了 新的位置Xnew
x 为预测, z 为观测
显然 k 决定了各自的权重,那么 k 又是如何确定呢?
我们就看谁更准,怎么做呢?
比如现在真实的位置我们知道,然后拿仪器测,如果结果一样,那说明仪器准,此外由于存在系统误差,如仪器不灵光,外部温度不同,电量不足等等,总之会有一定的误差,每次测量结果可能不太一样,那就需要多测几次,看看结果准不准,也就是计算方差大小,方差大小决定了可信度;
同样的方法人也一样,可以获得人的可信度;
显然 k 就是可信度占比
但是要注意一点,可信度随着状态的转移会发生变化,如人在一个熟悉的环境中,他判断位置就会比较准确,在一个陌生的环境中,判断位置就不太可信;
此时 新的位置Xnew 似乎确定了,但是别忘了,仍然是 “推测” 的,也不完全准确,也存在一定的偏差,此时的偏差也跟 k 有关,因为 k 实际上决定了 Xnew 的精度。
数学建模
卡尔曼滤波有两个假设
1. 世界上噪声无处不在,状态本身有噪声,状态转移也有噪声 ====> 比如现在的测量温度是 30 度,实际上可能是29度,这就是状态噪声;你开空调1分钟,可能变成25度,也可能变成26度,这就是状态转移噪声
2. 一个系统会处于各种不同的状态,并且可以相互转换,我们无法获取当前状态,但是可以通过测量猜测出当前状态
场景描述
有一辆小车在行进,车上装有传感器获取 速度v,也装有定位装置获取 位置p,那么小车的状态就可以用 x=[p, v]’ 来表示;
在车行驶的过程中,存在内部控制,如踩油门加速;也存在外部影响,如风阻
u 可以理解为加速度;
######## 得到 某时刻的状态
误差表示
卡尔曼滤波假设噪声无处不在,仪器获取的数据当然也有噪声,所以我们只能认为 测量值是真实值的最优估计,并且误差服从正态分布,
我们用 最优估计 代表当前状态,但是实际上真实的状态是某个范围中的一点
xk就是最优估计,其实就是均值,这里记住状态是均值;
其周围的部分就是方差,由于是两个量,车速和位置,所以用协方差表示
这就是当前状态的方差,即当前状态的可信度
######## 得到 该时刻的状态 的方差,形成 该时刻状态 的 高斯分布
状态转移
根据物理公式可以得到 下一时刻的 位置pt 和 速度vt,并转换成矩阵的表示方式,在形式上做简化,就得到如下公式。
其中 Ft 代表状态转移矩阵;Bt 代表状态控制矩阵;ut代表 状态控制向量
状态-均值发生变化,其方差也会相应转变
Pt = FtPt-1FtT,T表示转置 【协方差矩阵的性质 cov(Ax, Bx)=Acov(x, y)BT】
形成了新的高斯分布
考虑到外部影响因素,这里假设外部影响因素服从 高斯分布 wk~N(0, Qk) 【一会顺风,一会逆风,一会上坡,一会下坡,可以看做均值为0的正态分布】
至此得到完整的状态预测方程
状态是均值,由于 wk 均值为0,有些教程直接忽略了,具体使用时根据实际情况而定;
######## 得到 下一时刻的状态 的预测 和 对应的方差,形成 下一时刻状态 的 高斯分布,并带噪声
状态转换成指标
小车的状态是 车速 和 位置,但是状态可能不是我们需要的数学指标,所以需要进行转换,如观测到你出汗了,但我们需要你热不热,此时就把出汗转换成热;
假设这个转换矩阵为 Hk;
上步得到的 下一时刻状态的 预测和方差就要转换成
此时我们完成了下一状态的预测,结果服从正态分布
######## 得到 下一时刻指标 的预测 和 对应的方差,形成 下一个时刻指标的 高斯分布,带噪声
状态观测
用传感器观测小车的状态,车速和位置,也是带有误差的,转换成数学指标,
把 协方差 记作 Rk,把状态的数学指标记为 Zk,即均值
权衡分析
现在有了 下一时刻 的预测值和观测值,和对应方差,如下图
这就可以根据方差来确定 k (形象描述里面的k),然后计算这个时刻的 最优估计状态,并更新方差
这个 k 是个很重要的概念,叫卡尔曼增益。
为了简化式子,把上面三个式子都 左乘 Hk-1
第三个式子再 右乘 Hk 的转置的逆,得到如下式子
这就得到了 下一个时刻 的状态和噪声
此时就可以继续预测下下时刻的状态 和噪声。
总结
卡尔曼滤波就是5个公式
预测,更新,更新完在预测
图形描述
实例
模拟预测小车速度和位置的一段程序
观测值是一段从0到99,速度为1的匀速行驶路径,加入高斯噪声
import numpy as np import matplotlib.pyplot as plt ### 创建有噪声的观测值 # 创建一个0-99的一维矩阵 z = [i for i in range(100)] z_watch = np.mat(z) # 创建一个方差为1的高斯噪声,精确到小数点后两位 noise = np.round(np.random.normal(0, 1, 100), 2) noise_mat = np.mat(noise) # 将z的观测值和噪声相加 z_mat = z_watch + noise_mat ### 定义初始状态 # 定义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) p_mat = (np.eye(2) - kalman * h_mat) * p_predict print(x_mat) plt.plot(x_mat[0, 0], x_mat[1, 0], 'r*', markersize = 3) plt.show()
由于我们的观测值只有里程,0-99,而观测状态是 [里程,车速],所以转换矩阵 H 为 [1, 0],乘完之后只有里程。
输出
横坐标为路程,纵坐标为车速。
参考资料:
https://zhuanlan.zhihu.com/p/39912633 更详尽请参考该教程
https://www.zhihu.com/question/22422121/answer/22877882
https://blog.csdn.net/guo1988kui/article/details/82778198