卡尔曼滤波的理解、推导和应用

1. 卡尔曼滤波简介
2. 卡尔曼滤波原理
3. 卡尔曼滤波的公式
- 3.1. 五个公式
- 3.2. 公式作用
4. 卡尔曼滤波的公式推导
- 4.1. 符号说明
- 4.2. 概念说明
- - 4.2.1 高斯分布
  - 4.2.2 协方差矩阵
- 4.3. 更新优化的状态估计值
- 4.4. 求优化预测状态
- 4.5. 求卡尔曼增益
- 4.6. 求预测误差协方差矩阵
- 4.7. 求误差协方差矩阵
- 4.8. 加入外界对系统的作用
5. 卡尔曼滤波应用

1. 卡尔曼滤波简介

目的：对线性系统状态的估计

卡尔曼滤波（Kalman filtering）一种利用线性系统状态方程，通过系统输入输出观测数据，对系统状态进行最优估计的算法。由于观测数据中包括系统中的噪声和干扰的影响，所以最优估计也可看作是滤波过程。
斯坦利·施密特(Stanley Schmidt)首次实现了卡尔曼滤波器。卡尔曼在NASA埃姆斯研究中心访问时，发现他的方法对于解决阿波罗计划的轨道预测很有用，后来阿波罗飞船的导航电脑使用了这种滤波器。

2. 卡尔曼滤波原理

方法：利用状态过程中的噪声ω 和观测噪声υ ，给予合理的权重，对状态x 进行估计

详解的原文链接：http://www.bzarg.com/p/how-a-kalman-filter-works-in-pictures/
对此，进行一些简单的描述，只求通俗易懂：
假如想开发了一个可以在树林里到处跑的小机器人，那么需要知道它实时所在的位置，才能导航。
可以在机器人上装GPS定位装置，精度大约为10米，
但树林里有很多沟壑和悬崖，如果机器人走错了一步，就有可能掉下悬崖，所以这远远不够。

此时还可以获取一些运动的信息:
　　添加里程计和惯性惯性测量单元记录运动的过程，判断出机器人的姿势和行走距离。
　　获取发送给电机的指令，知道机器人是否在朝一个方向移动并且没有人干预，预测在下一个状态，机器人很可能朝着相同的方向移动。
　　
但由于传感器的精度、树林里的坑坑洼洼和自然界的风等影响，里程计和惯性测量单元实际并不能非常精确表示机器人实际姿势和行走距离，预测也不是很完美。

GPS 、里程计和惯性测量单元等传感器告诉了我们一些状态信息，我们的预测告诉了我们机器人会怎样运动，但都只是间接的，并且伴随着一些不确定和不准确性。
但是，如果使用所有对我们可用的信息，根据其本身的噪声，分配一定的权重，就能得到一个比任何依据自身估计更好的结果，这就是卡尔曼滤波的用处。

3. 卡尔曼滤波的公式

3.1. 五个公式

先了解一下卡尔曼滤波的五个公式：

3.2. 公式作用

公式（一）：计算预测状态值
公式（二）：计算预测值和真实值之间的预测误差协方差矩阵
公式（三）：在公式一和公式二的条件下，求得卡尔曼增益
公式（四）：计算估计值
公式（五）：计算估计值和真实值之间的误差协方差矩阵

4. 卡尔曼滤波的公式推导

4.1. 符号说明

在推导前，为了在推导过程中混淆，先做一个符号说明：

4.2. 概念说明

4.2.1 高斯分布

通过概率密度函数来定义高斯分布：
高斯分布的概率密度函数是：

均值为μ，标准差为σ
而上述的过程噪声ω 和观测噪声υ皆服从高斯分布。

4.2.2 协方差矩阵

方差：各个样本与样本均值的差的平方和的均值，度量一组数据的分散程度。

协方差：两个变量总体误差的期望，度量两个变量线性相关性程度。
当两个变量是相同的情况，协方差就是方差。
如果两个变量的变化趋势相反，其中一个变量大于自身的期望值时，另外一个却小于自身的期望值，那么两个变量之间的协方差就是负值。
两者之间的联系或者关系，关系越大，协方差越大。

协方差矩阵：数据集中两两变量的协方差组成，每个元素是各个向量元素之间的协方差

噪声协方差矩阵越小说明噪声的误差越小，可信度越高，其对角线上的值就是方差。
误差协方差矩阵越小说明过程噪声和量测噪声的关系越小，用比例分开过程噪声ω 和观测噪声υ，如果关系越小，分开的越精确。比如一堆白砂糖和盐，如果两种混合的很均匀，说它关系很大，也就越难用比例的方法将其分开。

4.3. 更新优化的状态估计值

假定k时刻的系统状态与k-1时刻有关，并且内部有噪声，则

4.4. 求优化预测状态

在无噪声情况下的预测

观测新息反应了预测噪声ω和观测噪声υ对状态的综合影响

将过程噪声ω_k 看作一个新息 * 比例，则

其中公式(6)，仅卡尔曼增益K_k为未知

4.5. 求卡尔曼增益

来看误差协方差矩阵：

而卡尔曼增益K_k应该使误差协方差矩阵P_k最小
状态新息（状态 - 预测状态）：

假设在无噪声情况下，状态新息:

则误差协方差矩阵：

此时，来看看预测误差协方差矩阵：

此时，再把误差协方差矩阵展开：

若使用T表示误差协方差矩阵的对角线，则

对T[P_k ]求导，就可找到最小的均方差，使T[P_k ]最小，就能得到卡尔曼增益K_k
对T[P_k ]求导：

令其为0，则卡尔曼增益：

其中观测矩阵H和观测噪声协方差矩阵R为常数，所以卡尔曼增益K_k与预测误差协方差矩阵P_k^’ 有关

4.6. 求预测误差协方差矩阵

假设观测矩阵 H = 1，那么卡尔曼增益：

则P_k^’ 越大，K_k越大，权重重视反馈测量
则P_k^’ 越小，K_k越小，权重重视预测值

此时，来看看下一状态的预测误差协方差矩阵：

因为状态x 和过程噪声ω_k是独立的，所以再简化

其中状态转移矩阵A和预测噪声协方差矩阵Q为常数，所以预测误差协方差矩阵P_k^’与上一状态的误差协方差矩阵P_k-1有关

4.7. 求误差协方差矩阵

4.8. 加入外界对系统的作用

在加入外界对系统的作用时，可整理出卡尔曼滤波的五个公式：

5. 卡尔曼滤波应用

用python来简单说明

import numpy as np
import matplotlib.pyplot as pltdelta_t = 0.1                               # 每秒钟采一次样
end_t = 7                                   # 时间长度
time_t = end_t * 10                         # 采样次数
t = np.arange(0, end_t, delta_t)            # 设置时间数组u = 1                                       # 定义外界对系统的作用 加速度
x = 1 / 2 * u * t ** 2                      # 实际真实位置v_var = 1                                   # 测量噪声的方差
# 创建高斯噪声，精确到小数点后两位
v_noise = np.round(np.random.normal(0, v_var, time_t), 2)X = np.mat([[0], [0]])                      # 定义预测优化值的初始状态
v = np.mat(v_noise)                         # 定义测量噪声
z = x + v                                   # 定义测量值（假设测量值=实际状态值+噪声）
A = np.mat([[1, delta_t], [0, 1]])          # 定义状态转移矩阵
B = [[1 / 2 * (delta_t ** 2)], [delta_t]]   # 定义输入控制矩阵
P = np.mat([[1, 0], [0, 1]])                # 定义初始状态协方差矩阵
Q = np.mat([[0.001, 0], [0, 0.001]])        # 定义状态转移(预测噪声)协方差矩阵
H = np.mat([1, 0])                          # 定义观测矩阵
R = np.mat([1])                             # 定义观测噪声协方差
X_mat = np.zeros(time_t)                    # 初始化记录系统预测优化值的列表for i in range(time_t):# 预测X_predict = A * X + np.dot(B, u)        # 估算状态变量P_predict = A * P * A.T + Q             # 估算状态误差协方差# 校正K = P_predict * H.T / (H * P_predict * H.T + R)     # 更新卡尔曼增益X = X_predict + K * (z[0, i] - H * X_predict)       # 更新预测优化值P = (np.eye(2) - K * H) * P_predict                 # 更新状态误差协方差# 记录系统的预测优化值X_mat[i] = X[0, 0]plt.rcParams['font.sans-serif'] = ['SimHei']    # 设置正常显示中文
plt.plot(x, "b", label='实际状态值')             # 设置曲线数值
plt.plot(X_mat, "g", label='预测优化值')
plt.plot(z.T, "r--", label='测量值')
plt.xlabel("时间")                               # 设置X轴的名字
plt.ylabel("位移")                               # 设置Y轴的名字
plt.title("卡尔曼滤波示意图")                     # 设置标题
plt.legend()                                    # 设置图例
plt.show()                                      # 显示图表

运行上面的卡尔曼滤波，得到下图：

可以看出，尽管测量值波动很大，但最终的预测优化值与实际状态值相差不大。

[1] python的代码地址:
https://github.com/JoveH-H/A-simple-explanation/blob/master/Kalman_filtering.py
[2] jupyter notebook的代码地址:
https://github.com/JoveH-H/A-simple-explanation/blob/master/ipynb/Kalman_filtering.ipynb

参考：
卡尔曼滤波 – 从推导到应用(一)
卡尔曼滤波 – 从推导到应用(二)
卡尔曼滤波算法–核心公式推导导论
详解卡尔曼滤波原理

谢谢！

卡尔曼滤波的理解、推导和应用相关推荐

【转】卡尔曼滤波算法详细推导（相当值得一看）
转载自卡尔曼滤波算法详细推导这一篇对预备知识的介绍还是很好的,过程与原理讲解也很到位,应该是目前看到中文里最好的讲解了. 一.预备知识 1.协方差矩阵是一个维列向量,是的期望 ...
离散时间傅里叶变换（理解推导）
文章目录傅立叶级数和傅立叶变换离散时间傅立叶变换推导 (DTFT) DTFT和CTFT的比较 Tips: 此贴适合有一定傅里叶变换和傅里叶级数基础的人观看.意在帮助大家更好的理解DTFT和CTFT ...
扩展卡尔曼滤波的理解与对加入高斯噪声的正弦信号进行滤波实例
文章目录卡尔曼滤波拓展卡尔曼滤波正弦波滤波卡尔曼滤波卡尔曼滤波是一个线性状态估似算法,对高斯噪声有良好的滤波效果,也可做多传感器融合算法,如九轴.具体推导网络上,网络上很多.珠玉在前我就不过 ...
卡尔曼滤波含义理解与公式推导(原作者王一哲，兰景公司，景贤计划)
卡尔曼滤波是数据融合处理以及现代信号处理中十分重要,但是读书的时候在我电过于注重形式,对其内涵认识不足,工作之后才发现廖桂生老师在矩阵分析的课程上说的太对了,其实对于数学的定义的理解以及公式背后的含义 ...
卡尔曼滤波原理理解（1）
一.什么是卡尔曼滤波简单来说,卡尔曼滤波器是一个"optimal recursive data processing algorithm(最优递归数据处理算法)". 在自然界中往 ...
神经网络中的激活函数与损失函数深入理解推导softmax交叉熵
神经网络中的激活函数与损失函数&深入理解softmax交叉熵前面在深度学习入门笔记1和深度学习入门笔记2中已经介绍了激活函数和损失函数,这里做一些补充,主要是介绍softmax交叉熵损失函数 ...
Python创建推导式，理解推导式【详细】
关于卡尔曼滤波详细推导的理解
相信大家到了关注卡尔曼滤波原理推导的阶段,已经对卡尔曼滤波有所了解了,大概知道,卡尔曼滤波是应用在含有噪声的.模型(预测)基本知道一些的.传感器测量含有噪声的这种场合,对真实的状态(或变量值)进行最优 ...
我所理解的卡尔曼滤波——公式推导与应用
我所理解的卡尔曼滤波--公式推导与应用 1.什么是卡尔曼滤波 2.卡尔曼滤波的数学推导 2.1 状态方程和测量方程 2.2 卡尔曼滤波过程 3 卡尔曼滤波应用 1.什么是卡尔曼滤波先举个例子说一下什 ...

卡尔曼滤波的理解、推导和应用

卡尔曼滤波的理解、推导和应用

1. 卡尔曼滤波简介

2. 卡尔曼滤波原理

3. 卡尔曼滤波的公式

3.1. 五个公式

3.2. 公式作用

4. 卡尔曼滤波的公式推导

4.1. 符号说明

4.2. 概念说明

4.2.1 高斯分布

4.2.2 协方差矩阵

4.3. 更新优化的状态估计值

4.4. 求优化预测状态

4.5. 求卡尔曼增益

4.6. 求预测误差协方差矩阵

4.7. 求误差协方差矩阵

4.8. 加入外界对系统的作用

5. 卡尔曼滤波应用

卡尔曼滤波的理解、推导和应用相关推荐

最新文章

热门文章