yangbo18416 / kalmanfilterav

针对编码器速度的卡尔曼滤波

1、概述

这是一个针对编码器采集的速度的卡尔曼滤波。原汁原味的卡尔曼滤波代码参考:

https://github.com/TKJElectronics/KalmanFilter，是关于mpu6050之类的imu的滤波算法

详细的推到过程参考:

http://blog.tkjelectronics.dk/2012/09/a-practical-approach-to-kalman-filter-and-how-to-implement-it/

有关卡尔曼滤波和卡尔曼增益的详细推导过程，目前我所找到的最好的教程参考B站：

CAN神 https://www.bilibili.com/video/BV1ez4y1X7eR

kalmanfilter.ods是用LibreOffice做的可视化图表，用EXCEL打开就行。

2、推导过程

系统状态$x_k$

上一时刻的后验估计状态用${\hat{x}}{k-1|k-1}$表示；先验状态( priori state)用${\hat{x}}{k|k-1}$表示，先验状态是根据上一个状态估计出的先验值；后验状态(posteriori state)用${\hat{x}}_{k|k}$表示；$x_k$代表真实值。

系统k时刻的状态为：

其中 state matrix：

其中v是速度，a是加速度，速度的导数。

状态转移矩阵 state transition model:

$w_k$是0均值的高斯分布的过程噪声：

$Q_K$是过程误差协方差矩阵：

$Q_k$协方差矩阵依赖于时间，时间越久误差越大，$Q_k$设置的越大，状态估计的噪声会越多，设置对于状态估计的置信度，可以通过更改Excel中这两个参数观察曲线变化，关于$Q_k$的值初始化参考kalman滤波理解三：协方差矩阵的计算_JK-CSDN博客_误差协方差矩阵

测量值$z_k$

在观测空间observation 获取测量值$z_k$和真实值$x_k$：

本文中的观测量是速度v，其中$v_k$是测量噪声measurement noise，$H$是观测模型observation model

观测噪声符合高斯分布：

$R$是测量噪声协方差矩阵，可通过测一组传感器数据获取或者传感器说明书获取，此处就等于$v$的方差：

$R$越小，对于数据的跟随性越好，越大，平滑度越好，更改观察Excel表中数据和图。

接下来是两个主要步骤，预测和校正

预测

下面是先验误差状态协方差矩阵priori error covariance matrix:

此矩阵代表对于当前估计值的置信度，这里$P$是一个2x2矩阵：

校正

首先计算测量值与通过先验状态观测值的差，可正可负，叫做innovation，革新？

观测模型$H$ 的作用是，将先验状态${\hat{x}}_{k|k-1}$映射到传感器测量值所在的观测空间

计算innovation协方差矩阵：

同理，$H$的作用是将先验误差协方差矩阵映射到观测空间。测量值的噪音越大，则$S$越大，代表我们不太相信新的测量值。

接下来是卡尔曼增益Kalman gain，指明我们相信innovation的程度：

例如我们不太相信innovation而是相信先验估计值，则$P$会比较小，$S$会比较大。

如果不知到初始状态值可以设置误差协方差矩阵为：

如果已知初始状态：

然后更新后验状态：

最后更新后验误差协方差矩阵posteriori error covariance matrix:

其中$I$是单位对角矩阵。

3、具体公式推导