卡尔曼滤波（Kalman filter)算法以及Arduino应用-mpu6050（导航贴）

正在更新中。。。

这篇文章要跟大家一起完全搞明白卡尔曼滤波，连一个标点符号也不放过，完完全全理解明白。

如果你看不懂，那说明我写的不好。

本文是看了dr_con博士的视频后做的，建议可以去看看。

视频链接：https://space.bilibili.com/230105574/channel/collectiondetail?sid=6939

目录

正在更新中。。。

        卡尔曼滤波究竟是在做什么？（数据融合）

        卡尔曼滤波融合什么数据？（先验估计与测量估计）

H矩阵的意义是什么？（传感器测量值与实际值的转化）

引例及思路

​编辑

推导卡尔曼增益K 

什么是过程噪声协方差矩阵和测量噪声协方差矩阵（Q矩阵和R矩阵的意义是什么）？

---

卡尔曼滤波究竟是在做什么？（数据融合）

卡尔曼滤波本质就是在搞数据融合。接下来我用初中公式来解释一下如何融合数据。

可能很多人不知道什么是数据融合，为了把这篇文章写明白。

所以我先用通俗的语言解释一下数据融合。

比如说我现在想称一下自己的体重，买了一台称，称了一下发现自己体重是63.5kg。

但是我从称上下来的时候，发现在上面完全没有站人的情况下，竟然显示0.3kg，于是我知道这个称不准确。

所以我又买了一个称，用新买的称称重之后发现自己的体重是60.0kg。

我知道世界上没有任何一台称是完全准确的，于是我取了平均值来当作我的真实体重。

如下图这样，我得到了我的体重是61.75kg。

取平均值的过程，就是将两个测量数据进行数据融合，得到了一个相对准确的估计数据。 

已经搞明白了什么是数据融合，接下来来弄明白卡尔曼核心公式之一——卡尔曼增益的意义。

通过取两次测量数据的平均值，我得到了一个新的更准确的体重值。

但是经验告诉我，事实不是这么简单，毕竟如果我取平均值的话，那说明把两台称放在了同等地位去考虑，但现实不总是这样的。

比如有的称更好他就更准确，有的称不太好就没那么准，我们倾向于在更准的数据上面加权。所以我打算取这两个数的加权平均数来当作我的真实体重。

那么如何加权呢？

第一台称我在拼多多上面花29.9包邮买的。

第二台称我在京东上面花69.9买的。

于是根据一分价钱一分货的道理，我本能觉得贵的更好。

于是我这样做。

这就是对不同的称进行加权处理，比如上图中第一个称权重是30%，第二个称权重是70%，也就是说我对第一个值考虑了30%，对第二个值考虑了70%。通过这种加权的方式将两个数据进行了融合。

但是实际上我用称的价格进行权重考虑也不是很准确，毕竟购买的东西不总是一份价格一分货。

你不能说我在网上9.9元包邮买的耳机音质效果是4.9元包邮买的耳机的两倍吧！

于是我考虑设第一个称的权重为K，那么第二个称的权重自然为1-K。

然后再通过一种方法（这个方法在下文，这里先不纠结）来找到一个合适的K，给两个数进行加权求得加权平均数。

如下图：

我把卡尔曼滤波五大核心公式放在下面：

 我们发现，求两个称加权平均数的过程，跟卡尔曼进行后验估计的公式很像，简直一样。

这就对了，因为卡尔曼滤波中进行后验估计的过程，就是在取先验估计与测量估计的加权平均数。

即对先验估计与测量估计两个数据进行融合（通过求加权平均数融合两个数据）得到后验估计。

而这个合适的K即权重，就是卡尔曼增益。

由于我们刚刚称重是在讨论一维数据，因此，K就是个数字。

而如果上升到多维数据，那么K就变成了矩阵了。

卡尔曼增益K矩阵——权重矩阵。

刚刚上面又说了三个新的概念，后验估计、先验估计和测量估计。

这个我们下面再解释，同时配合解释H矩阵的意义，这样理解的更深刻。

我们这里就理解了卡尔曼滤波的真正意义，其实就是在两个不太准确的值之间求加权平均数进行数据融合，这个权重即卡尔曼增益。

卡尔曼滤波融合什么数据？（先验估计与测量估计）

上面我们搞明白了卡尔曼滤波的本质目的，就是通过数据融合得到一个相对准确的数据。

那么我们在卡尔曼滤波中，到底是在对哪些数据进行数据融合呢？

先验估计与测量估计！

H矩阵的意义是什么？（传感器测量值与实际值的转化）

很多同学没有学过控制理论，而大部分文章写半天都没写这些矩阵到底是啥意思，也不说这些矩阵如何求得。

本着让大家一起搞明白的原则，我们来解释一下。

引例及思路

推导卡尔曼增益K 

 

 计算Pk的先验矩阵

 

什么是过程噪声协方差矩阵和测量噪声协方差矩阵（Q矩阵和R矩阵的意义是什么）？

Q是过程噪声协方差矩阵，那什么是过程噪声？什么是过程噪声协方差矩阵？

很多文章说Q矩阵和R矩阵靠经验设置或者测量得到，但是你倒是告诉我怎么靠经验得到，或者怎么测量出来啊！说了跟没说一样。今天我们一定要搞明白这两个矩阵如何精确得跑到手掌心。

比如我有一个mpu6050，他集成了三轴加速度和三轴角速度传感器，能够输出这六个状态量.

但是我实际工作时由于是做的平衡小车，因此只需要知道一个倾角就行。

所以我现在只专注于mpu6050传感器的一个倾角。

过程噪声就是模型估计值的误差。

mpu6050库

// class default I2C address is 0x68
// specific I2C addresses may be passed as a parameter here
// AD0 low = 0x68 (default for SparkFun breakout and InvenSense evaluation board)
// AD0 high = 0x69

#include "Wire.h"          //I2C通讯库
#include "I2Cdev.h"        //
#include "MPU6050.h"       //mpu6050库

MPU6050 mympu;             //定义mympu对象

float pi=3.1415926;             //Π的值，用于单位转换
float AcceRatio=16384.0;        //加速度计比例系数
float GyroRatio=131.0;          //陀螺仪比例系数             
//定义3个变量，用于存储倾角数据
float angle_x=0.0,angle_y=0.0,angle_z=0.0;
int16_t ax=0,ay=0,az=0,gx=0,gy=0,gz=0;     //加速度计陀螺仪原始数据
float accx=0.0,accy=0.0,accz=0.0;

void setup(){   
  Wire.begin(18, 5, 400000);//开启iic通讯，mpu6050的数据传输协议为iic
  Serial.begin(115200);//打开串口
  mympu.initialize();  //初始化mpu6050
}

void loop() {
    //通过调用该函数，一次性从mpu6050获取6轴数据
    mympu.getMotion6(&ax,&ay,&az,&gx,&gy,&gz);
    accx=ax/AcceRatio;              //x轴加速度
    accy=ay/AcceRatio;              //y轴加速度
    accz=az/AcceRatio;              //z轴加速度
    angle_x=(atan(accx/accz)*180/pi); //计算身体前后的倾角，绕y轴的转角
    angle_y=(atan(accy/accz)*180/pi); //计算身体左右的倾角，绕x轴的转角
    Serial.print(az);Serial.print("     ");//将z轴加速度原始数据输出
    Serial.print(accx);Serial.print("     ");//将3轴加速度输出（单位:g)
    Serial.print(accy);Serial.print("     ");//将两个转角从串口输出
    Serial.print(accz);Serial.print("     ");//将两个转角从串口输出
    Serial.print(angle_x);Serial.print("     ");//将两个转角从串口输出
    Serial.print(angle_y);Serial.println("   ");
    delay(100);
}