通俗的解释卡尔曼滤波（Kalman Filter）以及其Python的实现

卡尔曼滤波

风力发电机中的风速估计，转速估计甚至扭矩估计都设计到卡尔曼滤波，如果只是单一传感变量的平滑处理也能用到卡尔曼滤波。
振动信号中的滤波大多采用低通去除高频噪音，而卡尔曼滤波则是通过不确定度把置信度高的值滤出来。综合看了不少文献和资料，能把卡尔曼滤波讲清楚的最好的几篇分别是：
Understanding the Basis of the Kalman Filter Via a Simple and Intuitive Derivation
https://zhuanlan.zhihu.com/p/45238681
https://www.zhihu.com/question/23971601
https://zhuanlan.zhihu.com/p/64539108
http://www.bzarg.com/p/how-a-kalman-filter-works-in-pictures/

本质上来说，工业上对对象的动态认知主要来自两方面，一方面是基于之前状态和动力学理论进行的的状态推导，另一中方法是直接通过传感器读取状态栏。遗憾的是不管是间接推导还是直接测量的结果都有一定的误差，分别来自推导的不确定度和传感器本身的不确定度。如何融合推导和直接测量得出最优估计就是卡尔曼滤波针对不确定信息的动态系统所做的事情，简单来说就是谁的结果可靠性高些就信的更多点，在推导结果的正太分布和传感器测量结果的正太分布求个并集，推导出新的正太分布，这个正太分布的平均值就是最优估计。

'Understanding the Basis of the Kalman Filter Via a Simple and Intuitive Derivation’这篇文章用几幅图比较生动的解释了这一点：

这是一个小车的运动状态示意图，有个传感器可以直接观测到小车的状态（距离S，速度V），同时也有运动学方程可以根据上一时刻小车的速度和距离预测一个当前小车的状态量，当然这两种方法都是有一定的误差，并且符合正太分布：

卡尔曼滤波是一种降噪手段，目的是减少预测的噪声和测量的噪声，得到最优估计，也就是下图中绿色的概率分布均值：

好了，下面贴上卡尔曼滤波典型的几个方程：

第一个方程

是状态预测公式，矩阵 Ft 为状态转移矩阵，表示如何从上一状态来推测当前时刻的状态;Bt为控制矩阵，表示控制量 ut如何作用于当前矩阵；xt 有顶帽子，表示只是估计值，并不是最优的:


状态预测公式只能告诉我们状态预测的均值，前面说过预测也是正太分布，因此还需要协方差矩阵P，假设前一时刻Pt-1已知，那么t时刻的协方差矩阵表示为：

上式中Q是一些无法确认的干扰噪音。

对于传感器的读数分布也是需要方程来进行表述：

Ht为观测矩阵，v为观测噪音，很多文章和博客写H矩阵是用于将传感器读取的数据尺度转换为我们关系的状态，但实际从英文原文中应该指的是将预测的值转换到传感器观测预的矩阵，这样才能在同一尺度上做两者的融合。从测量到的传感器数据中，我们大致能猜到系统当前处于什么状态。但是由于存在不确定性，某些状态可能比我们得到的读数更接近真实状态。我们将这种不确定性（例如：传感器噪声）用协方差Rt 表示,即上式中的v，该分布的均值就是我们读取到的传感器数据，称之为Zt。

K即为卡尔曼增益，主要作用为协调预测协方差矩阵和观测协方差矩阵，或者说为预测和观测分配权重。

利用卡尔曼增益进行状态量的更新，综合考虑预测和观测，得到：

最后我们还要对协方差矩阵Pt进行更新用作下一次迭代：

总结起来，卡尔曼滤波主要是以下5个公式：

前两个表示的是根据上一时刻的状态来预测当前时刻的状态，通过这两个公式我们得到的是非最佳估计的x和P；后三个公式就是通过当前的观测值来更新x和P，更新之后的就是最佳观测值。

直接点说，只要能写出P，Q，R，H和F矩阵的动态系统，都能用卡尔曼滤波解决动态状态估计问题。

下面是我用Python简单测试下风速仪测试的风速结果卡尔曼滤波估计的效果：

import pandas as pd
import matplotlib.pyplot as plt
import numpy as np
import math
import glob
import natsort

plt.rcParams['font.sans-serif']=['SimHei'] #用来正常显示中文标签
plt.rcParams['axes.unicode_minus']=False #用来正常显示负号

file_path = 'D:/Works/ten_avg_val.csv'
df = pd.read_csv(file_path, index_col = False, engine = 'python').dropna()
df['时间'] = pd.to_datetime(df['时间'])
df = df.set_index('时间')

n_iter = 10000
sz = (n_iter,) # size of array
# x = -3*np.sin(np.arange(n_iter)) # truth value (typo in example at top of p. 13 calls this z)
z = np.array(df['风速'][0:n_iter]) # observations (normal about x, sigma=0.1)

plt.plot(z)
plt.show()

Q = 1e-5 # process variance

# allocate space for arrays
xhat=np.zeros(sz)      # a posteri estimate of x
P=np.zeros(sz)         # a posteri error estimate
xhatminus=np.zeros(sz) # a priori estimate of x
Pminus=np.zeros(sz)    # a priori error estimate
K=np.zeros(sz)         # gain or blending factor

R = 0.1**2 # estimate of measurement variance, change to see effect

# intial guesses
xhat[0] = 0.0
P[0] = 1.0

for k in range(1,n_iter):
    # time update
    xhatminus[k] = xhat[k-1]
    Pminus[k] = P[k-1]+Q

    # measurement update
    K[k] = Pminus[k]/( Pminus[k]+R )
    xhat[k] = xhatminus[k]+K[k]*(z[k]-xhatminus[k])
    P[k] = (1-K[k])*Pminus[k]

plt.figure()
plt.plot(z,'k-',label='noisy measurements')
plt.plot(xhat,'r-',label='a posteri estimate')
plt.legend()
plt.title('Estimate vs. iteration step', fontweight='bold')
plt.xlabel('Iteration')
plt.ylabel('temp')
plt.show()

如果将测量不确定度R改为接近于0，取R=0.001 ** 2,则卡尔曼滤波认为传感器测的非常真实，降噪结果也和观测结果很一致，如下图：

如果将测量不确定度R改为较大，取R=1 ** 2,则卡尔曼滤波认为传感器测的真实性降低，降噪结果则会更平滑，即离观测值较远，如下图：