Kalman Filter --卡尔曼滤波

简介

卡尔曼滤波（Kalman filtering）是一种利用线性系统状态方程，通过系统输入输出观测数据，对系统状态进行最优估计的算法。由于观测数据中包括系统中的噪声和干扰的影响，所以最优估计也可看作是滤波过程。

数据滤波是去除噪声还原真实数据的一种数据处理技术，Kalman滤波在测量方差已知的情况下能够从一系列存在测量噪声的数据中，估计动态系统的状态。由于它便于计算机编程实现，并能够对现场采集的数据进行实时的更新和处理，Kalman滤波是目前应用最为广泛的滤波方法，在通信，导航，制导与控制等多领域得到了较好的应用。

实例解析

已知上个时刻飞机的位置，知道现在这个时刻收到的雷达测量的飞机的位置。用前面两个数据来估计此时飞机的位置。精简的说就是知道上个时刻状态，又知道测量数据，融合这两个数据来求当前状态。

因为雷达接受的数据与飞机实时位置数据有延时误差，所以即使收到测量数据但是还是不确飞机位置在哪。于是我得需要根据前一个时刻的位置估计出当前时刻的飞机位置 结合 测量数据 综合考虑来 估计当前飞机位置。这就是卡尔曼滤波的作用。

卡尔曼认为所有的状态变化（位置变化）都是线性的。什么叫做线性？即飞机作匀速直线运动。如若状态位置变化不是线性的如飞机做变速运动又该如何？其实此情况已被发明了新的算法：扩展卡尔曼滤波。这里我们只研究卡尔曼滤波。

滤波算法简介

滤波算法本质上就是利用多个数据来融合估计真实状态。

在这里插入图片描述

设有个飞机只会水平飞行。已知上个时刻 $t_1$ 飞机的位置 $x_1$ 和速度 $v_1$ 。雷达探测到 $t_2$ 时的位置 $x_2^*$ 。则有两种方法得到飞机位置：

1：飞机匀速运动（ $v_2 = v_1$ ),现在的位置： $x_1 + v_2(t_2-t_1)$ 。

2：位置就是 $x_2^*$ 。

由于飞机速度可变，雷达测量不准，我们认为飞机在时刻 $t_2$ 的位置为:
$x^{real}_2 =(x_1 + v_2(t_2-t_1)) + \alpha[x^*_2-(x_1 + v_2(t_2-t_1))]$
$\alpha$ 时雷达测量的置信度，若雷达不准将α 赋值很低如0.1.如果雷达很准那么就设置α 很接近1的值如0.9。你会发现α越接近0，那么所估计的位置越是接近认为飞机匀速走的那个思路。α越接近1那么所估计的位置越接近雷达测量值（等于认为飞机在时刻$ t_2$的位置就是雷达测量值）。
————————————————
滤波算法就是为了确定α具体取值的计算方法。

在卡尔曼滤波中：α是用来调节根据上个位置估计出的当前位置值与测量值在最终结果中的占比用的。这个在卡尔曼滤波中叫做卡尔曼增益即 $K_{n}$ 。

在这里插入图片描述

$K_n$ 即是调节最终结果 $x^{real}$ 值靠近估计值和测量值中的哪一个。

现在我们得到了新的位置估计值，那么根据这个来估计下一个位置它的方差 $p_{n-1}$ 是多少呢？由于根据上个状态算出当前状态在最终结果的占比占了 $1 -K_n$ 。所以方差 $p_{n}=(1-k_n)p_{n-1}$

卡尔曼滤波最关键的是需要求出两种方差。一是纯粹依赖估计的这种方法的方差。二是测量仪器的方差。只要知道了这两个值那就可以知道估计值和测量值在最终结果中的占比。算方差的方法在不同应用场景是不同的需要以实际情况而定。

Python编程实践卡尔曼滤波


import numpy as np

# 模拟数据
t = np.linspace(1, 100, 100)
a = 0.5
position = (a * t ** 2) / 2

position_noise = position + np.random.normal(0, 120, size=(t.shape[0]))
import matplotlib.pyplot as plt

plt.plot(t, position, label='truth position')
plt.plot(t, position_noise, label='only use measured position')

# 初试的估计飞机的位置就直接用雷达测量的位置
predicts = [position_noise[0]]
position_predict = predicts[0]

predict_var = 0
odo_var = 120 ** 2  # 这是我们自己设定的位置测量仪器的方差，越大则测量值占比越低
v_std = 50  # 测量仪器的方差
for i in range(1, t.shape[0]):
    dv = (position[i] - position[i - 1]) + np.random.normal(0, 50)  # 模拟从IMU读取出的速度
    position_predict = position_predict + dv  # 利用上个时刻的位置和速度预测当前位置
    predict_var += v_std ** 2  # 更新预测数据的方差
    # 下面是Kalman滤波
    position_predict = position_predict * odo_var / (predict_var + odo_var) + position_noise[i] * predict_var / (
                predict_var + odo_var)
    predict_var = (predict_var * odo_var) / (predict_var + odo_var) ** 2
    predicts.append(position_predict)

plt.plot(t, predicts, label='kalman filtered position')

plt.legend()
plt.show()

在这里插入图片描述

借鉴：若想深造请移步