蓝牙RSSI定位入门到精通（5）-卡尔曼滤波

下篇文章惯性室内导航入门到精通（1）-加速度传感器
https://blog.csdn.net/qq_35651984/article/details/82845525

前言

学习卡尔曼滤波需要知道的知识
高斯分布，高斯白噪音，均值，方差，协方差矩阵

卡尔曼滤波有什么用

卡尔曼滤波可以有效预测向量的下一次走向，并不断优化回归，使预测更精确。总之就是：预测+自我调整。在这里，可以拿来预测室内运动的走向，减少干扰。

什么是卡尔曼滤波

卡尔曼是匈牙利数学家。我们学习的正是源于他的博士论文和1960年发表的论文《A New Approach to Linear Filtering and Prediction Problems》如果对这编论文有兴趣，可以到这里的地址下载：http://www.cs.unc.edu/~welch/kalman/media/pdf/Kalman1960.pdf

举个栗子

大家好，我是曹冲，今天孙权送了一头大象过来，父王想知道大象的重量。于是我想了个办法，把象放到大船上，在水面所达到的地方做上记号，再让船装载其他东西（当水面也达到记号的时候），称一下这些东西,比较下。但在这个时代的秤误差很大，况且大象在船上蹦蹦跳跳，水面起起伏伏，小河也有水波，误差很大。而我有强迫症，有没有办法得到更精确的重量呢？？
在这里插入图片描述

这时候，博主告诉我。你可以先估计下一次测量的重量。根据我多年秤象的经验，误差大概在400斤左右。由于大象的重量是不变的，这头比较肥，所以我估计有1500斤，估计的误差为500斤（由于估计误差和秤象误差叠加，误差会增大，500平方=300平方+400平方）。然后用刻舟秤石头，获得大象重量1600斤，误差300斤。这时候出来两个重量，我该相信那个重量呢？
在这里插入图片描述
这时候，博主又跳出来了。你可以计算两个重量的权重。预测的权重为kg=0.609（预测的平方/预测的平方+秤象的平方2500/2500+1600）(注，在卡尔曼中kg应该为平方，不用开根号)。最后我们能得到估计的实际的重量 1500斤+0.609*（1600-1500）=1560斤
不是说卡尔曼是预测+自我调整吗？所以我还要修改我的预测值。
（(1-kg)*这次估计误差的平方）最后整体开方,得到下次的估计误差316斤，下次我就用316斤来计算，这样，连续秤象n次，就可以得到大象精确的重量了。

这就是卡尔曼的核心的原理，只要搞懂下面的五个公式，就能基本理解卡尔曼滤波了。
在这里插入图片描述
在这里我就不详细介绍了。只要理解上面的栗子，很快就能学会。
推荐的卡尔曼详解：https://blog.csdn.net/u010720661/article/details/63253509

代码

创建Kalman类以及实例化。

用卡尔曼滤波实现定位中，主要用于过滤RSSI。

class Kalman
{
public:
    Kalman(double Q, double R);
    double  KalmanFilter(double value);
private:
    double startValue;  //k-1时刻的滤波值，即是k-1时刻的值
    double kalmanGain;   //   Kalamn增益
    double A;   // x(n)=A*x(n-1)+u(n),u(n)~N(0,Q)
    double H;   // z(n)=H*x(n)+w(n),w(n)~N(0,R)
    double Q;   //预测过程噪声偏差的方差
    double R;   //测量噪声偏差，(系统搭建好以后，通过测量统计实验获得)
    double P;   //估计误差协方差;
};
#endif // KALMAN_H

     kalman_1=new Kalman(9.0,100.0);//预测误差的方差,噪声误差的方差
     kalman_2=new Kalman(9.0,100.0);
     kalman_3=new Kalman(9.0,100.0);

构造函数

Kalman::Kalman(double Q, double R)//预测误差的方差,噪声误差的方差
{//初始化
      A=1;
      H=1;
      P=10;//下一时刻的协方差，初始化随意
      this->Q=Q;
      this->R=R;
      startValue=50;
}

运算

double Kalman::KalmanFilter( double value)//传入测量值
{
    //预测下一时刻的值
    double predictValue = A* startValue;

    //求预测下一时刻的协方差
    P = A*A*P + Q;  //计算先验均方差 p(n|n-1)=A^2*p(n-1|n-1)+q
   //计算kalman增益
    kalmanGain = P*H / (P*H*H + R);  //Kg(k)= P(k|k-1) H’ / (H P(k|k-1) H’ + R)
    //修正结果，即计算滤波值
    startValue = predictValue + (value - predictValue)*kalmanGain;  //利用残余的信息改善对x(t)的估计，给出后验估计，这个值也就是输出  X(k|k)= X(k|k-1)+Kg(k) (Z(k)-H X(k|k-1))
    //更新后验估计
    P = (1 - kalmanGain*H)*P;//计算后验均方差  P[n|n]=(1-K[n]*H)*P[n|n-1]
    return  startValue;
}

最后，在需要滤波的位置，放置卡尔曼算法

                        rssi[0]=kalman_1->KalmanFilter(rssi[0]);
                        rssi[1]=kalman_2->KalmanFilter(rssi[1]);
                        rssi[2]=kalman_3->KalmanFilter(rssi[2]);