卡尔曼滤波的理解
Posted 非晚非晚
tags:
篇首语：本文由小常识网(cha138.com)小编为大家整理，主要介绍了卡尔曼滤波的理解相关的知识，希望对你有一定的参考价值。
文章目录
1. 卡尔曼滤波概念
卡尔曼滤波（Kalman filtering）是一种利用线性系统状态方程，通过系统输入输出观测数据，对系统状态进行最优估计的算法。由于观测数据中包括系统中的噪声和干扰的影响，所以最优估计也可看作是滤波过程。简单来说，卡尔曼滤波器是一个“optimal recursive data processing algorithm（最优化自回归数据处理算法）”
- 卡尔曼滤波思想：
以 K − 1 K - 1 K−1时刻的最优估计 X k − 1 X_k-1 Xk−1为准，预测 K K K时刻的状态变量 X k ∣ k − 1 Xk|k-1 Xk∣k−1，同时又对该状态进行观测，得到观测变量 Z k Z_k Zk，
再对预测与观测之间进行分析，或者说是以观测量对预测量进行修正，从而得到K时刻的最优状态估计 X k X_k Xk。
- 状态转移方程： x k = A x k − 1 + B u k − 1 + w k − 1 x_k=Ax_k-1+Bu_k-1+w_k-1 xk=Axk−1+Buk−1+wk−1
这个状态方程是根据上一时刻的状态和控制变量来推测此刻的状态， w k − 1 w_k-1 wk−1是服从高斯分布的噪声，也称为过程噪声，是预测过程的噪声，它对应了 x k x_k xk 中每个分量的噪声，是期望为 0，协方差为 Q 的高斯白噪声 w k − 1 w_k-1 wk−1 ~ N(0,Q)，Q即激励噪声。A为状态转移矩阵，B为控制输入矩阵。
- 观测方程： z k = H x k + v k z_k= Hx_k+v_k zk=Hxk+vk
v k v_k vk是观测的噪声，服从高斯分布，也称为测量噪声， v k v_k vk~N(0,R),R测量噪声。H为状态观测矩阵。
2. 卡尔曼滤波理解
（1）理解例子一
以机器人举例障碍物距离为例。现在已知“传感器测量的机器人离障碍物的距离”，“上个时刻机器人离障碍物距离”和“自己当前时刻的速度”这三个数据。
- 观测值：传感器测量的机器人与障碍物的距离，比如雷达直接测量机器人离障碍物距离7m；
- 估计值：假如上一秒离障碍物10m，速度是4m/s，那么现在这秒估计就离障碍物距离是6m。
如果雷达测量的那个7m准确度是90%，根据速度估计出的那个6m准确度是80%，那么最终的距离估计结果就是：
r
e
s
u
l
t
=
(
1
−
0.9
0.8
+
0.9
)
∗
6
+
0.9
0.8
+
0.9
∗
7
=
6.52
m
result = (1-\\frac0.90.8+0.9)*6+\\frac0.90.8+0.9*7= 6.52m
result=(1−0.8+0.90.9)∗6+0.8+0.90.9∗7=6.52m
其中
0.9
0.8
+
0.9
\\frac0.90.8+0.9
0.8+0.90.9为卡尔曼增益，它就是表示这个传感器数据相对于根据速度计算出的估计值的靠谱程度。
（2）理解例子二
下面例子是为了加深对卡尔曼滤波过程的理解。如果上一例子看懂了，下面例子也可以跳过。
- 假设我们要研究的对象是一个房间的温度。根据你的经验判断，这个房间的温度是恒定的，也就是下一分钟的温度等于现在这一分钟的温度（假设我们用一分钟来做时间单位）。假设你对你的经验不是100%的相信，可能会有上下偏差几度。我们把这些偏差看成是高斯白噪声（White Gaussian Noise），也就是这些偏差跟前后时间是没有关系的而且符合高斯分配（Gaussian Distribution）。另外，我们在房间里放一个温度计，但是这个温度计也不准确的，测量值会比实际值偏差。我们也把这些偏差看成是高斯白噪声。
- 假如我们要估算k时刻的是实际温度值。首先你要根据k-1时刻的温度值，来预测k时刻的温度。因为你相信温度是恒定的，所以你会得到k时刻的温度预测值是跟k-1时刻一样的，假设是23度，同时该值的高斯噪声的偏差是5度（5是这样得到的：如果k-1时刻估算出的最优温度值的偏差是3，你对自己预测的不确定度是4度，他们平方相加再开方，就是5）。然后，你从温度计那里得到了k时刻的温度值，假设是25度，同时该值的偏差是4度。
- 由于我们用于估算k时刻的实际温度有两个温度值，分别是23度和25度。究竟实际温度是多少呢？相信自己还是相信温度计呢？究竟相信谁多一点，我们可以用他们的covariance来判断。因 K g 2 = 5 2 / ( 5 2 + 4 2 ) Kg^2=5^2/(5^2+4^2) Kg2=52/(52+42)，所以Kg=0.78，我们可以估算出k时刻的实际温度值是：
23+0.78*(25-23)=24.56度。可以看出，因为温度计的covariance比较小（比较相信温度计），所以估算出的最优温度值偏向温度计的值。- 现在我们已经得到k时刻的最优温度值了，下一步就是要进入k+1时刻，进行新的最优估算。到现在为止，好像还没看到什么自回归的东西出现。对了，在进入k+1时刻之前，我们还要算出k时刻那个最优值（24.56度）的偏差。算法如下： ( 1 − K g ) ∗ 5 2 = 2.35 \\sqrt(1-Kg)*5^2=2.35 (1−Kg)∗52=2.35。这里的5就是上面的k时刻你预测的那个23度温度值的偏差，得出的2.35就是进入k+1时刻以后k时刻估算出的最优温度值的偏差（对应于上面的3）。
- 就是这样，卡尔曼滤波器就不断的把covariance递归，从而估算出最优的温度值。他运行的很快，而且它只保留了上一时刻的covariance。
上面的Kg，就是卡尔曼增益（Kalman Gain）。他可以随不同的时刻而改变他自己的值，是不是很神奇！
3. 卡尔曼滤波的计算过程
还是以第二节中的"理解一"为例进行说明。
首先我们先回顾总结下直观理解中是怎么做的。
- 根据上一秒导弹的位置 和 导弹的的速度估计出当前时刻导弹的位置粗略估计值。
- 将雷达测得导弹位置测量值和我们计算出的导弹位置粗略估计值根据这两种数据可信度来进行线性加权和得到准确的导弹位置估计值。
在前面我们也提到了导弹的位置和雷达测量值都是有误差的。所以卡尔曼想用概率来衡量数据的可信度。
卡尔曼认为导弹速度、导弹位置、雷达测距的测量值这些都服从正态分布。我们用 x t − 1 x_t-1 xt−1表示导弹在t-1时刻的位置的概率分布（它画出来就和上面那个图长的一样）。 v t − 1 v_t-1 vt−1表示导弹在t-1时刻的速度的概率分布。用 z t z_t zt表示雷达测量到导弹离目标的距离概率分布。并且它们都是正态分布。
现在我们已知
- 导弹在t-1时刻的位置的概率分布，为 x t − 1 = N ( 10 , 0. 2 2 ) x_t-1=N(10,0.2^2) xt−1=N(10,0.22)。其实0.2就是上个时刻导弹位置的误差
- 导弹的速度的概率分布，为 v t − 1 = N ( 4 , 0. 7 2 ) v_t-1=N(4,0.7^2) vt−1=N(4,0.72)。其实0.7就是获取速度的那个传感器噪声误差。你会问咋就有噪声误差了？不知道你有没有用过手机里面的指南针那个软件，明明你没动那个指南针还是会在某个范围内波动。还有称体重，明明你没动那个称数据就是不断的动。这就是噪声误差。
雷达在t时刻测量到导弹离目标的距离的概率分布，为 z t = N ( 7 , 0. 1 2 ) z_t=N(7,0.1^2) zt=N(7,0.12)，这0.1就是雷达的噪声误差。
在前面我们知道了卡尔曼滤波算法为了融合雷达测量值和上个时刻的导弹状态数据来减少误差，需要实施“两步走”战略。
- 根据上一秒导弹的位置 和 导弹的速度估计出当前时刻导弹的位置粗略估计值。
- 将雷达测得导弹位置测量值和我们计算出的导弹位置粗略估计值根据这两种数据可信度来进行线性加权和得到准确的导弹位置估计值。
我们来看看具体怎么做的。
- 计算粗略估计值（这个是做了一个硬假设：导弹是保持匀速运动），大家可以对比着看下直观理解是怎么做的：(注意这里的估计是一秒间隔，下面的时间是乘以1s的)
x 在 t 时 刻 的 粗 略 估 计 值 = x t − 1 − v t − 1 = N ( 10 , 0. 2 2 ) − N ( 4 , 0. 7 2 ) = N ( 6 , 0. 2 2 + 0. 7 2 ) x在t时刻的粗略估计值=x_t-1-v_t-1=N(10,0.2^2)-N(4,0.7^2)=N(6,0.2^2+0.7^2) x在t时刻的粗略估计值=xt−1−vt−1=N(10,0.22)−N(4,0.72)=N(6,0.22+0.72)
- 根据粗略估计值的概率分布与雷达的测量值概率分布得到精确估计值的概率分布。根据贝叶斯滤波所推导，直接把两个概率分布相乘即可：
x t = x 在 t 时 刻 的 估 计 值 ∗ z t = N ( 6 , 0. 2 2 + 0. 7 2 ) ∗ N ( 7 , 0. 1 2 ) = N ( 6 , 0.5 3 2 ) ∗ N ( 7 , 0. 1 2 ) x_t=x在t时刻的估计值*z_t=N(6,0.2^2+0.7^2)*N(7,0.1^2)=N(6,0.53^2)*N(7,0.1^2) xt=x在t时刻的估计值∗zt=N(6,技术分享 | 运动状态估计之卡尔曼滤波详解