第一章 单维卡尔曼滤波以及实例应用

这是小白博主第一次发博文，文中有误的地方欢迎各位大佬批评指正，小白一定会向各位大佬虚心学习！

此文章需要有卡尔曼滤波基础知识的储备（因为文中并没有对具体公式推导作详细的说明）

卡尔曼滤波能做什么呢？这是一个很好的问题，其实呢，卡尔曼滤波能做的事情很多，这里呢，我参考了多位博主的文章从而得出结论，这里就以我学习的经验来向各位朋友分享我的收获与心得，并且呢，这里我将使用卡尔曼滤波作用在超声波模块测距实例上，对超声波模块测量得到的距离作未滤波与滤波后波形的这样一个对比。

废话不多说，咱们先直接上波形图

注：蓝色是未滤波之前的波形，橙色为滤波之后的波形

一、卡尔曼滤波算法的五个基本公式

1.预测现在的状态

x(k|k-1) = A*x(k-1|k-1) + B*U(k)

解释：x(k-1|k-1)是上一时刻的最优解；

           x(k|k-1)是利用上一状态预测的结果；

           U(k)为现在状态的控制量，一般情况下没有，如果没有，可以为0；

           A是作用在x(k-1|k-1)下的状态转移矩阵，它是算法对状态变量进行预测的依据。由于这里是一维数据，所以A为1；

           B是作用在控制量上的控制矩阵，在大多数实际情况下并没有控制增益 ；

           一维数据下，系统预测值 = 系统状态变量k-1时刻的最优解。因此呢，上面的公式可以写成  x(k|k-1) = x(k-1|k-1)，稍加修改，超声波实验里，我们用到的公式是：

X_mid = X_last;

以上便是卡尔曼滤波的第一个方程

2.更新协方差

p(k|k-1) = A*p(k-1|k-1) *A'+ Q;

解释：p(k|k-1)是x(k|k-1)对应的协方差；

           p(k-1|k-1)是x(k-1|k-1)对应的协方差；

           A是作用在x(k-1|k-1)下的状态转移矩阵，它是算法对状态变量进行预测的依据。由于这里是一维数据，所以A为1，A'为A的转置矩阵，这里为1；

           Q为过程噪声；

           因此最后得到的公式为：p(k|k-1) = p(k-1|k-1) + Q，实验中咱们用到的公式是：

P_mid = P_last + Q;

3.计算当前的卡尔曼增益

Kg(k) = p(k|k-1)*H'/(p(k|k-1)+R);

解释：Kg(k)为卡尔曼增益；

          R为传感器的噪声平均值；

          p(k|k-1)是x(k|k-1)对应的协方差；

          实验用到的是一维数据，因此H'为1；

          最终得到的公式为：Kg(k) = p(k|k-1)/(p(k|k-1)+R);

          实验中咱们用到的公式为：Kg = P_mid/(P_mid+R);

4.修正估计值，得出最优解

x(k|k) = x(k|k-1) + Kg(k)*(Z(k) - x(k|k-1));

解释：x(k|k)为当前修正值，得到的当前最优解；

           Z(k)为传感器测得的测量值；

           Kg(k)为卡尔曼增益；

           x(k|k-1)是利用上一状态预测的结果；

           实验中咱们用到的公式为：X_now = X_mid + Kg*(Z_mearure-X_mid);

5.计算最优解所对应的协方差

p(k|k) = (1 - Kg(k)*H) * p(k|k-1);

解释：p(k|k)为此时最优解得到的协方差

           由于是一维数据，H为1；

           实验中咱们用到的公式为：P_now = (1 - Kg)*P_mid;

注意：以上公式为卡尔曼滤波的五个公式，前两个为预测方程，后三个为更新方程

二、卡尔曼滤波c程序实现

6.超声波实验卡尔曼滤波程序

超声波程序很简单，这里咱们只展示卡尔曼滤波代码

/*卡尔曼参数*/
 double R = 0.5;//系统测量噪声
  double Q = 0.5;//系统过程噪声
   double X_mid;//利用上一状态预测的结果
	double X_last = 0;//上一刻的最优状态值 ，初始值为0
	 double X_now;//系统当前预测值和测量值估计的最优状态值
	  double Kg;//卡尔曼增益
	   double P_mid;//利用上一状态结果计算的协方差
		double P_last = 1;//上一刻最优预测值的协方差
		 double P_now;//这一刻对应的最优协方差
		  double Z_mearure;//超声波的测量值
/*卡尔曼滤波，mearure为超声波测量值入口参数*/
double Kalman_Filter(double mearure)
{
	  Z_mearure = mearure;
	 
      X_mid = X_last;//1.预测先验估计值
	  P_mid = P_last + Q;//2.预测当前先验估计值所对应的协方差

	  Kg = P_mid/(P_mid+R);//3.更新卡尔曼增益
	  X_now = X_mid + Kg*(Z_mearure-X_mid);//修正先验估计值的最优值
	  P_now = (1 - Kg)*P_mid;//更新当前状态所对应的协方差
	    
	  //更新下一次的协方差和预测值
	  X_last = X_now;//
	  P_last = P_now;//
	    
	  //返回最优解
	  return X_now;
}