【教程】卡尔曼滤波的详解与C语言实现

一、什么是卡尔曼滤波

卡尔曼滤波（Kalman filtering）是一种利用线性系统状态方程，通过系统输入输出观测数据，对系统状态进行最优估计的算法。由于观测数据中包括系统中的噪声和干扰的影响，所以最优估计也可看作是滤波过程。
数据滤波是去除噪声还原真实数据的一种数据处理技术，Kalman滤波在测量方差已知的情况下能够从一系列存在测量噪声的数据中，估计动态系统的状态。由于它便于计算机编程实现，并能够对现场采集的数据进行实时的更新和处理，Kalman滤波是目前应用最为广泛的滤波方法，在通信，导航，制导与控制等多领域得到了较好的应用。

二、如何理解 卡尔曼滤波

在理解卡尔曼之前我们先讲个故事，比如现在我们要测一个房间的温度，那么我们需要一个温度计。假设市场上买两种温度计，一种测得温度非常准没有误差。另一种只能大概测量。当然测得准的温度计，比较贵，另外一个比较便宜。如果你是土豪那么你可以买比较贵的就是没有误差的，那么你就跟卡尔曼没有故事了。
现在假设你是一个中等屌丝，你可以卖得起两个破的温度计，现在两个温度计测量得到两个值，我们假设他一个是23度，一个是25度，那么实际应该是多少度呢。如果你对他们求平均（即权值是0.5,0.5），你会得到24度左右。再说的明确一点呢24±1度，应为这个是你猜的，所以你不能肯定，那么这个1度就是你的不确定度。可能你是天蝎座的，比较偏心，不想然两个温度计都雨露均沾，我们假设你比较相信前一个温度计，不相信另一个，这里我们取权值为（0.7，0.3），那么你得到的温度值就是0.7*23+0.3*25=23.6度，那么不确定度可能是1（感觉自己偏心的有道理，猜的比较准），可能是6度（这个6是随便写的，猜的不准误差比较大）。根据上面这个问题我们发现
1.不确定我们猜的准不准，换句话来说就是不确定度不知道多少
2.权值该怎么取才合理
省略1W字。。。。。。。
更多请查看：https://blog.csdn.net/CBQ5789789/article/details/78858160

---------------------------------------------

最近做的项目用到iPhone收集的蓝牙信号强度，即RSSI值。发现果然如文献中所说，即使手机保持静止，检测到的来自同一蓝牙信标的信号强度也会不停地上下波动，且数据（滤除粗大误差后）呈高斯分布。许多文献中也提到，对付这样的噪声可以用卡尔曼滤波器。同样地，做自平衡车、四旋翼等用到的陀螺仪、加速度数据也常常用到卡尔曼滤波

省略1W字。。。。。。。
更多请查看：https://www.jianshu.com/p/c512a2b82907

---------------------------------------------

https://blog.csdn.net/von_kent/article/details/76610952

---------------------------------------------

三、C语言实现 卡尔曼滤波

/*
 ============================================================================
 Name        : KaerMan.c
 Author      : 
 Version     :
 Copyright   : Your copyright notice
 Description : Hello World in C, Ansi-style
 ============================================================================
 */

#include <stdio.h>
#include <stdlib.h>



static double p_last = 0;
static double x_last = 0;

//过程噪音   买的破温度计有多破。。
#define P_Q 0.1
//测量噪声
#define M_R 0.01
/*
        Q:过程噪声，Q增大，动态响应变快，收敛稳定性变坏
        R:测量噪声，R增大，动态响应变慢，收敛稳定性变好

        其中p的初值可以随便取，但是不能为0（为0的话卡尔曼滤波器就认为已经是最优滤波器了）
q,r的值需要我们试出来，讲白了就是(买的破温度计有多破，以及你的超人力有多强)

r参数调整滤波后的曲线与实测曲线的相近程度，r越小越接近。

q参数调滤波后的曲线平滑程度，q越小越平滑。

*/
static double KalmanFilter(const double ResrcData)
{

    double R = M_R;
    double Q = P_Q;

    double x_mid = x_last;
    double x_now;

    double p_mid ;
    double p_now;

    double kg;

    //这里p_last 等于 kalmanFilter_A 的p直接取0
    x_mid=x_last;                       //x_last=x(k-1|k-1),x_mid=x(k|k-1)
    p_mid=p_last+Q;                     //p_mid=p(k|k-1),p_last=p(k-1|k-1),Q=噪声

    /*
      *  卡尔曼滤波的五个重要公式
      */
    kg=p_mid/(p_mid+R);                 //kg为kalman filter，R 为噪声
    x_now=x_mid+kg*(ResrcData-x_mid);   //估计出的最优值
    p_now=(1-kg)*p_mid;                 //最优值对应的covariance
    p_last = p_now;                     //更新covariance 值
    x_last = x_now;                     //更新系统状态值

    return x_now;
}

float kalmanFilter_A(float inData)
{
  static float prevData=0;
  //其中p的初值可以随便取，但是不能为0（为0的话卡尔曼滤波器就认为已经是最优滤波器了）
  static float p=0.01, q=P_Q, r=M_R, kGain=0;
    p = p+q;
    kGain = p/(p+r);

    inData = prevData+(kGain*(inData-prevData));
    p = (1-kGain)*p;

    prevData = inData;

    return inData;
}

int main(void) {
	double datas[] = {99, 33, 45, 99, 100, 24};
	int len = sizeof(datas)/ sizeof(datas[0]);

	for(int i = 0; i < len; i++){
		printf("datas[i]=%lf, number=%lf, %f\n", datas[i], KalmanFilter(datas[i]), kalmanFilter_A(datas[i]));
	}

	return 0;
}

我这里摘录了两种不同写法的卡尔曼的实现代码。（实现都相同，写法不同。结果不同，因为p的取值不同）

运行结果为：

datas[i]=99.000000, number=90.000000, 90.750000
datas[i]=33.000000, number=37.786260, 37.846153
datas[i]=45.000000, number=44.394619, 44.399647
datas[i]=99.000000, number=94.417504, 94.417923
datas[i]=100.000000, number=99.531516, 99.531548
datas[i]=24.000000, number=30.338621, 30.338623