卡尔曼滤波记录

滤波一般也是比较常用的知识了，滤波对于数据的稳定性特别重要，要想得到稳定的数据，滤波是必不可少的一个步骤，之前在参加各种比赛的时候就广泛用到了滤波的方法，正好最近的毕设也需要用一下，这里就顺便记录下这个过程。

注：本文多有参考，如有雷同，那肯定是我抄了你的。

1、卡尔曼滤波理论推导

卡尔曼滤波算法的推导网上太多了，这里我就不继续赘述了，我只记录一点关键的东西，先用一个比较清晰的例子说明卡尔曼滤波是什么吧：

认为一个系统，比如小车，小车是由电机来驱动的对吧，我们只要改变电机的转速，小车就会转起来，那这个转速的给定我们简单的认为他和PWM是成正比的：

那么就可以有一个理想的情况：

这里$x_k\text{和}{x}_{k-1}$是小车的两个物理量（比如速度），当匀速的时候可以认为一直是1，但是实际情况下这样的状态是不存在的，因为小车存在打滑，本身的非线性，以及我们本身统计这个小车速度的误差等诸多因素

这里我们引入了$v_k$这个测量噪声这个变量 ，理论上来说$v_k$只跟传感器有关，我们用差一点的传感器，测量噪声就小，反之测量噪声就比较大，但不管怎样，这个测量噪声都是不可避免的。

下面对上面的物理量计算进行更新：

这里的$b{u}_k$就是噪声了，那么卡尔曼滤波的思想其实就是综合考虑之前的测量值和当前值进行比较，判断最合适的结果，下面可以得到卡尔曼的计算表达（一维情况）

这里，上面打了符号的，例如${\hat{x}}_k$都表示估计值综合利用上一次的估计值和测量值对最终结果进行判断，其中$g_k$称为卡尔曼增益（就是一个比例系数），这里这个系数的作用就是，当 $g_k$=1的时候，预测结果就是测量值，当 $g_k$=0的时候，预测结果就是估计值，所以它代表了我们对于这个测量结果的置信度。

但是这个又不全是这样的，如果全是这样的那不就是一阶低通滤波了吗，所以他这里$g_k$是在不断变化的。

然后就是五大公式了，五大公式网上也比较多讲的，这里也不赘述了，基本上也就是

• 状态预测
• 计算协方差
• 估计卡尔曼系数
• 计算估计值
• 更新协方差

这里为什么是协方差：因为在概率论里面我们可以知道就是变量的相关性可以用协方差来描述。

2、使用matlab进行卡尔曼滤波测试

下面的所有测试均使用正弦函数叠加噪声来进行测试：

这里我也是找了一段代码进行修改，重点还是关注Q，还有R，他们就是卡尔曼增益所代表的含义了，通过调节Q，R就能获取比较理想的效果了。

这里有一些参数要做一下说明（后面的python，c语言版本这里都是一样的）

这里认为是线性系统，所以基本上初值设置为1就都可以了，其他的初值为0即可，会根据实际情况在计算中更新。

R设置的比较小的时候可以看到数据比较漂，然后其实也更贴合实际

增大R，更相信估计值，这个时候数据比较圆滑，但是数据跟真实值有了一定偏移了

源码如下：

clc
clear

%% 参数设置
N = 500;     % 设置数据长度为N
t = (1:N);   % 生成时间轴
a = 1.01;       % 状态转移方程
b = 0;       % 控制输入
c = 1;       % c: 观测方程
x = 0;       % 设置初值 初始的均值和方差
sigma2 = 0;
Q = 0.01;     % 设置状态转移方差Q和观测方差R
R = 0.8;       

%% 初始化
real_signal = zeros(1,N); % 真实信号
z = zeros(1,N);           % 观测数据
x_filter = zeros(1,N);    % 存储卡尔曼估计的结果，缓存起来用于画图形
K = zeros(1,N);           % 存储卡尔曼预测中的增益k，缓存起来用于画图形

% 初始化真实数据和观测数据
for i=1:N
   real_signal(i) = 100*sin(5*2*pi*i/N); 
   z(i) = real_signal(i) + normrnd(0, 10);
end

%% 卡尔曼滤波
for i=1:N
    % 预测步
    x_ = a*x + b; %预测当前状态
    sigma2_ = a*sigma2*a'+Q;
    % 更新步
    k = sigma2_*c'/(c*sigma2_*c'+R);
    x = x_ + k*(z(i) - c*x_);
    sigma2 = (1-k*c)*sigma2_;
    % 存储滤波结果
    x_filter(i) = x;
    K(i) = k;
end
scatter(t,z,'b','+')
% legend('测量');
hold on;
plot(t, real_signal, 'r', t, x_filter, 'g')
legend('测量','real','filter');

运行可以看到结果如下所示

3、使用Python进行滤波

python这里思路其实和matlab是一样的，这里也是用公式进行推导编写的函数，同样注意关注这个Q和R，源码如下：

import numpy as np
import matplotlib.pyplot as plt

x_last = 0
p_last = 0
Q = 0.05  # 系统噪声
R = 0.8  # 测量噪声

def kalman(z_measure, x_last=0, p_last=0, Q=0.018, R=0.0542):
    x_mid = x_last
    p_mid = p_last + Q
    kg = p_mid / (p_mid + R)
    x_now = x_mid + kg * (z_measure - x_mid)
    p_now = (1 - kg) * p_mid
    p_last = p_now
    x_last = x_now
    return x_now, p_last, x_last

x1 = np.arange(0,200)
real = np.sin(np.linspace(0, 20, 200))
chao = np.random.rand(200)-0.5
x = real + chao
y = []
for i in range(len(x)):
    pred, p_last, x_last = kalman(x[i], x_last, p_last, Q, R)
    y.append(pred)

plt.plot(real, color="b") 
plt.scatter(x1, x, marker='x',color="g") 
plt.plot(y, color="r")  
plt.show()

实现效果：

后面发现python其实有内置函数，可以直接供我们进行调用，函数库的名字为pykalman，源码我是参考官方的源代码，如下所示：

import numpy as np
import matplotlib.pyplot as plt
from pykalman import KalmanFilter
from matplotlib.pylab import mpl

mpl.rcParams['font.sans-serif'] = ['SimHei']  # 显示中文
mpl.rcParams['axes.unicode_minus'] = False  # 显示负号

x1 = np.arange(0,200)
real = np.sin(np.linspace(0, 20, 200))
chao = np.random.rand(200)-0.5
x = real + chao

kf = KalmanFilter(transition_matrices=np.array([[1, 1], [0, 1]]),transition_covariance=0.001 * np.eye(2)) # 设置测量矩阵和转移矩阵
states_pred = kf.em(x).smooth(x)[0]

plt.scatter(x1,x,marker='x',color="g",label="smooth")
plt.plot(x1, real, color='b',label="real")
plt.plot(x1, states_pred[:, 0], color='r',label="kalman")

plt.legend()
plt.xlabel("x")
plt.ylabel("y")
plt.title("滤波效果")
plt.show()

不过这个库函数没有专门提供参数，我们只需要修改的协方差矩阵的系数就行了，如下所示

下面对系数进行修改，可以看到效果如下所示，下面R比较小的时候

调整参数，将R增大（这里适当减小Q是一样的）

4、在STM32上进行卡尔曼滤波测试

下面就到了使用c语言了，网上这个版本其实比较多，代码也都是大同小异，这里我采用的方法问为stm32输出一段正弦波叠加随机数上去，然后采用卡尔曼进行滤波。

工程这部分只需要配置下串口即可

下面加入卡尔曼初始化代码，这里也是Q和R的参数

可以插卡滤波效果如下所示

单独查看滤除后的波形

kalaman.c

#include "kalaman.h"

void KalmanFilter(Kalman_Typedef *klm, double input)
{
    //预测协方差方程：k时刻系统估算协方差 = k-1时刻的系统协方差 + 过程噪声协方差
    klm->Now_P = klm->LastP + klm->Q;
    //卡尔曼增益方程：卡尔曼增益 = k时刻系统估算协方差 / （k时刻系统估算协方差 + 观测噪声协方差）
    klm->Kg = klm->Now_P / (klm->Now_P + klm->R);
    //更新最优值方程：k时刻状态变量的最优值 = 状态变量的预测值 + 卡尔曼增益 * （测量值 - 状态变量的预测值）
    klm->out = klm->out + klm->Kg * (input -klm->out);//因为这一次的预测值就是上一次的输出值
    //更新协方差方程: 本次的系统协方差付给 kfp->LastP 威下一次运算准备。
    klm->LastP = (1-klm->Kg) * klm->Now_P;
}

void Kalman_Init(Kalman_Typedef *klm, const double klm_Q, const double klm_R)//温度klm_Q=0.01 klm_R=0.25
{
	klm->LastP=0.02;	//上次估算协方差
	klm->Now_P=0;			//当前估算协方差
	klm->out=0;				//卡尔曼滤波器输出
	klm->Kg=0;				//卡尔曼增益
	klm->Q=klm_Q;			//Q:过程噪声协方差 Q参数调滤波后的曲线平滑程度，Q越小越平滑;
	klm->R=klm_R;			//R:观测噪声协方差 R参数调整滤波后的曲线与实测曲线的相近程度，R越小越接近(收敛越快)
}

kalaman.h

typedef struct
{
  /*不用动*/
  double LastP;//上次估算协方差
  double Now_P;//当前估算协方差
  double out;//卡尔曼滤波器输出
  double Kg;//卡尔曼增益
  double Q;
  double R;
} Kalman_Typedef;

void Kalman_Init(Kalman_Typedef *klm, const double klm_Q, const double klm_R);
void KalmanFilter(Kalman_Typedef *klm, double input);