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 plt

delta_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

参考： 卡尔曼滤波 – 从推导到应用(一)：https://blog.csdn.net/heyijia0327/article/details/17487467# 卡尔曼滤波 – 从推导到应用(二)：https://blog.csdn.net/heyijia0327/article/details/17667341 卡尔曼滤波算法–核心公式推导导论：https://zhuanlan.zhihu.com/p/24312995 详解卡尔曼滤波原理：http://baijiahao.baidu.com/s?id=1604230489177143048&wfr=spider&for=pc