这篇文章要跟大家一起完全搞明白卡尔曼滤波，连一个标点符号也不放过，完完全全理解明白。
如果你看不懂，那说明我写的不好。
本文是看了dr_con博士的视频后做的，建议可以去看看。
如果哪里写的不对，欢迎批评指正。
视频链接：https://space.bilibili.com/230105574/channel/collectiondetail?sid=6939
目录
正在更新中。。。
卡尔曼滤波究竟是在做什么？（数据融合）
H矩阵的意义是什么？（传感器测量值与实际值的转化）
卡尔曼滤波融合什么数据？（先验估计与测量估计）
什么是过程噪声协方差矩阵和测量噪声协方差矩阵（Q矩阵和R矩阵的意义是什么）？
噪声
mpu6050小例程（读取一个角度）
mpu6050小例程（采集角度噪声）
mpu6050小例程（计算角度噪声的方差）
mpu6050小例程（读取角加速度）
mpu6050小例程（计算角加速度方差-即角加速度噪声方差）
mpu6050小例程（计算角度与角加速度噪声协方差）
引例及思路
编辑
推导卡尔曼增益K
卡尔曼滤波本质就是在搞数据融合。接下来我用初中公式来解释一下如何融合数据。
可能很多人不知道什么是数据融合，为了把这篇文章写明白。
所以我先用通俗的语言解释一下数据融合。
比如说我现在想称一下自己的体重，买了一台称，称了一下发现自己体重是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矩阵是什么。
很多同学没有学过控制理论，而大部分文章写半天都没写这些矩阵到底是啥意思，也不说这些矩阵如何求得。
本着让大家一起搞明白的原则，我们来解释一下。
我们用mpu6050的三轴加速度计来举例。
mpu6050的加速度计的三轴分量ACC_X、ACC_Y和ACC_Z均为16位有符号整数。
16位无符号整数的范围为0~2^16即0~65535，而16位无符号整数范围为-32768~32767.
而mpu6050加速度计有4个可选倍率：2g、4g、8g、16g。倍率默认为2g。假设我们设置的倍率为4g。
那么也就是说我们用单片机读取到-32768的加速度值时，也就是4g的加速度。
当读到32767这个数值的时候，也就是-4g的加速度。
我们定义以下三个2x1的矩阵。X(k)、Z(k)、V(k)。
X(k)是k时刻系统状态量，里面包含了x轴和y轴的实际加速度ax和ay。
Z(k)是k时刻传感器测量量，就是传感器测得的x轴和y轴的加速度数值zx和zy。
V(k)是k时刻传感器噪声量，就是传感器本身测量x轴或y轴加速度时的噪声vx和vy。
根据我们上面的知识，我们很容易得到下面A框中的式子，从测量值计算实际的加速度值。
经过变形之后得到B框中的式子。
然后我们把B框中的式子写成矩阵的形式，就变成了C框中的形式。
自然就明白了H矩阵的意义，H矩阵可以让我们很方便的在传感器测量值-32768-32767与实际加速度值-4g~4g之间进行多维转换（因为同时转换了x轴和y轴加速度，是两个维度同时转换）。
而C框中红框这个式子就是我们的测量模型。
测量模型反应传感器测量值与实际状态量之间的转换关系。
上面我们搞明白了卡尔曼滤波的本质目的，就是通过数据融合得到一个相对准确的数据。
那么我们在卡尔曼滤波中，到底是在对哪些数据进行数据融合呢？
先验估计与测量估计！
我们看下面的式子，A框里面的式子是我们的离散物理模型和测量模型。
C框里面就是我们进行估计后得到的先验估计和测量估计。
如果不懂的话，看下面小球自由落体运动的例子，这个是高中物理学的。
X(k)是小球现在的位移，X(k-1)是小球上一时刻的位移。
自由落体是匀加速的运动学模型，然后将噪声忽略变成理想化模型，也就是说这个模型不准确了。
然后靠着这个理想化模型我们进行小球的位移预测，我们估计到的这个位移数值就是先验估计。
所以就得到了先验估计。
然后我们看测量估计，这时我们用上面一节的mpu6050的那个例子。
C框中是测量模型，忽略噪声之后，变成了D框中的形式。而状态量X(k)也因为我们忽略了噪声不准确了，所以这个时候我们从传感器得到的是个测量估计。
在这一节，搞明白了什么是先验估计和测量估计。
总结一下：
通过忽略模型噪声，通过数学模型进行理论上的估计得到的就是先验估计。
通过忽略测量噪声，通过传感器测量数值进行估计得到的就是测量估计。
那么接下来就是通过卡尔曼增益将他们融合起来了，就是求取加权平均值。
Q是过程噪声协方差矩阵，那什么是过程噪声？什么是过程噪声协方差矩阵？
很多文章说Q矩阵和R矩阵靠经验设置或者测量得到，但是你倒是告诉我怎么靠经验得到，或者怎么测量出来啊！说了跟没说一样。今天我们一定要搞明白这两个矩阵如何精确得跑到手掌心。
比如我有一个mpu6050，他集成了三轴加速度和三轴角速度传感器，能够输出这六个状态量.
但是我实际工作时由于是做的平衡小车，因此只需要知道一个倾角就行。
所以我现在只专注于mpu6050传感器的一个倾角。
这里噪声我们理解成一个高斯分布。
这里介绍一个mpu6050小例程，因为接下来要边说边做实验了，所以先把实验搞明白。方便接下来采集数据，分离噪声，计算方差和协方差以及协方差矩阵等一系列操作。
mpu6050中，比较重要的两个倾角是俯仰角和横滚角，航向角通常代表前进的方向，所以暂时先不考虑航向角。
我们先只对俯仰角和横滚角中的一个倾角进行研究分析。
#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);
}#include "Wire.h" //I2C通讯库
#include "I2Cdev.h" //
#include "MPU6050.h" //mpu6050库
const byte MotorDriverEn = 5; //电机驱动器使能
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();//开启iic通讯，mpu6050的数据传输协议为iic
Serial.begin(115200);//打开串口
pinMode(MotorDriverEn,OUTPUT);
digitalWrite(MotorDriverEn,LOW);
mympu.initialize(); //初始化mpu6050
}
void loop() {
//通过调用该函数，一次性从mpu6050获取6轴数据
// Serial.println(millis());
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(ax);Serial.print(" ");//将z轴加速度原始数据输出
Serial.print(ay);Serial.print(" ");//将3轴加速度输出（单位:g)
Serial.print(az);Serial.print(" ");//将两个转角从串口输出
Serial.print(gx);Serial.print(" ");//将z轴加速度原始数据输出
Serial.print(gy);Serial.print(" ");//将3轴加速度输出（单位:g)
Serial.print(gz);Serial.print(" ");//将两个转角从串口输出
Serial.print(accz);Serial.print(" ");//将两个转角从串口输出
Serial.print(angle_x);Serial.print(" ");//将两个转角从串口输出
Serial.print(angle_y);Serial.println(" ");
// delay(100);
}#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(33, 32, 400000); //开启iic通讯，mpu6050的数据传输协议为iic
Serial.begin(115200); //打开串口
mympu.initialize(); //初始化mpu6050
}
void loop() {
float angle_ys[100],angle_yerr[100];
float angle_y_sum=0.0; //采集到的100个倾角的和
float angle_y_ave=0.0; //倾角平均值
float angle_yerr_sum=0.0;
float angle_yerr_ave=0.0; //噪声平均值
float angle_yerr_sigma2=0.0; //噪声方差
for(int i=0;i<100;i++)
{
//通过调用该函数，一次性从mpu6050获取6轴数据,ax,ay,az分别是x轴，y轴，z轴加速度;gx,gy,gz分别是角速度
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轴的转角
angle_ys[i]=angle_y;
angle_y_sum+=angle_y; //只研究angle_y
Serial.print(angle_y);Serial.print(" ");
delay(10);
}
Serial.println();
angle_y_ave=angle_y_sum/100.0;
for(int i=0;i<100;i++)
{
angle_yerr[i]=angle_ys[i]-angle_y_ave; //现在angle_yerr数组存的即为噪声
Serial.print(angle_yerr[i]);Serial.print(" ");
}
Serial.println();
delay(10000);
}用Baize_Servo1主板的代码：
#include "Wire.h" //I2C通讯库
#include "I2Cdev.h" //
#include "MPU6050.h" //mpu6050库
#define times 100
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(); //开启iic通讯，mpu6050的数据传输协议为iic
Serial.begin(115200); //打开串口
mympu.initialize(); //初始化mpu6050
}
void loop() {
float angle_ys[times],angle_yerr[times]; //采集100次倾角
float angle_y_sum=0.0; //采集到的100个倾角的和
float angle_y_ave=0.0; //倾角平均值
float angle_yerr_sum=0.0;
float angle_yerr_ave=0.0; //噪声平均值
float angle_yerr_sigma2=0.0; //噪声方差
for(int i=0;i<times;i++)
{
//通过调用该函数，一次性从mpu6050获取6轴数据,ax,ay,az分别是x轴，y轴，z轴加速度;gx,gy,gz分别是角速度
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轴的转角
angle_ys[i]=angle_y;
angle_y_sum+=angle_y; //只研究angle_y
Serial.print(angle_y);Serial.print(" ");
delay(10);
}
Serial.println();
angle_y_ave=angle_y_sum/100.0;
for(int i=0;i<times;i++)
{
angle_yerr[i]=angle_ys[i]-angle_y_ave; //现在angle_yerr数组存的即为噪声
Serial.print(angle_yerr[i]);Serial.print(" ");
}
Serial.println();
delay(10000);
}#include "Wire.h" //I2C通讯库
#include "I2Cdev.h" //
#include "MPU6050.h" //mpu6050库
#define times 100
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;
float grox=0.0,groy=0.0,groz=0.0;
void setup(){
Wire.begin(); //开启iic通讯，mpu6050的数据传输协议为iic
Serial.begin(115200); //打开串口
mympu.initialize(); //初始化mpu6050
}
void loop() {
float angle_ys[times],angle_yerr[times],gx_ys[times],gx_yerr[times],gy_ys[times],gy_yerr[times];
float angle_y_sum=0.0,gx_sum=0.0,gy_sum=0.0; //采集到的100个倾角的和
float angle_y_ave=0.0,gx_ave=0.0,gy_ave=0.0; //倾角平均值
float angle_yerr_sum=0.0;
float angle_yerr_ave=0.0; //噪声平均值
float angle_yerr_sigma2=0.0; //噪声方差
for(int i=0;i<times;i++)
{
//通过调用该函数，一次性从mpu6050获取6轴数据,ax,ay,az分别是x轴，y轴，z轴加速度;gx,gy,gz分别是角速度
mympu.getMotion6(&ax,&ay,&az,&gx,&gy,&gz);
accx=ax/AcceRatio; //x轴加速度
accy=ay/AcceRatio; //y轴加速度
accz=az/AcceRatio; //z轴加速度
grox=gx/GyroRatio;
groy=gy/GyroRatio;
groz=gz/GyroRatio;
angle_x=(atan(accx/accz)*180/pi); //计算前后的倾角，绕y轴的转角
angle_y=(atan(accy/accz)*180/pi); //计算左右的倾角，绕x轴的转角
angle_ys[i]=angle_y;
angle_y_sum+=angle_y; //
gx_ys[i]=grox;
gx_sum+=grox;
gy_ys[i]=groy;
gy_sum+=groy;
Serial.print(grox);Serial.print(" ");
delay(10);
}
Serial.println();
angle_y_ave=angle_y_sum/100.0;
gx_ave=gx_sum/100.0;
gy_ave=gy_sum/100.0;
for(int i=0;i<times;i++)
{
angle_yerr[i]=angle_ys[i]-angle_y_ave; //现在angle_yerr数组存的即为噪声
gx_yerr[i]=gx_ys[i]-gx_ave;
gy_yerr[i]=gy_ys[i]-gy_ave;
Serial.print(gx_yerr[i]);Serial.print(" ");
}
Serial.println();
delay(10000);
}#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(33, 32, 400000); //开启iic通讯，mpu6050的数据传输协议为iic
Serial.begin(115200); //打开串口
mympu.initialize(); //初始化mpu6050
}
void loop() {
float angle_ys[100],angle_yerr[100];
float angle_y_sum=0.0; //采集到的100个倾角的和
float angle_y_ave=0.0; //倾角平均值
float angle_yerr_sum=0.0;
float angle_yerr_ave=0.0; //噪声平均值
float angle_yerr_sigma2=0.0; //噪声方差
for(int i=0;i<100;i++)
{
//通过调用该函数，一次性从mpu6050获取6轴数据,ax,ay,az分别是x轴，y轴，z轴加速度;gx,gy,gz分别是角速度
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轴的转角
angle_ys[i]=angle_y;
angle_y_sum+=angle_y; //只研究angle_y
Serial.print(angle_y);Serial.print(" ");
delay(10);
}
Serial.println();
angle_y_ave=angle_y_sum/100.0;
for(int i=0;i<100;i++)
{
angle_yerr[i]=angle_ys[i]-angle_y_ave; //现在angle_yerr数组存的即为噪声
angle_yerr_sum+=angle_yerr[i];
Serial.print(angle_yerr[i]);Serial.print(" ");
}
Serial.println();
angle_yerr_ave=angle_yerr_sum/100.0;
//计算噪声方差
for(int i=0;i<100;i++)
angle_yerr_sigma2+=(angle_yerr[i]-angle_yerr_ave)*(angle_yerr[i]-angle_yerr_ave);
angle_yerr_sigma2=angle_yerr_sigma2/100.0;
Serial.println(angle_yerr_sigma2,4);
delay(10000);
}因为我们在arduino程序中从串口分别输出了三行数据，第一行是角度angle,第二行是噪声angle_error，第三行是噪声的方差。
所以，我们很方便的就可以把这些数据放到matlab中进行验证。我们发现angle和angle_error的方差一模一样，这就对了，因为angle本身包含真值和噪声，而真值就是这个时候的真实倾角，由于我们把mpu6050静止放置，所以这个是一个客观的定值，不会改变，所以他的方差肯定是0.所以angle的方差实际上就是完全受噪声影响的，跟噪声方差等价。
从matlab中绘的图也可以看出来。
过程噪声就是先验估计的误差。
三个角速度分量均以“度/秒”为单位，能够表示的角速度范围，即倍率可统一设定，有4个可选倍率：250度/秒、500度/秒、1000度/秒、2000度/秒。倍率默认设定为250度/秒。
#include "Wire.h" //I2C通讯库
#include "I2Cdev.h" //
#include "MPU6050.h" //mpu6050库
MPU6050 mympu; //定义mympu对象
float pi=3.1415926; //Π的值，用于单位转换
float AcceRatio=16384.0; //加速度计比例系数
float BetaRatio=32768.0; //加速度计比例系数
float GyroRatio=131.0; //陀螺仪比例系数
//定义3个变量，用于存储倾角数据
float angle_x=0.0,angle_y=0.0,angle_z=0.0;
float beta_x=0.0,beta_y=0.0,beta_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(33, 32, 400000); //开启iic通讯，mpu6050的数据传输协议为iic
Serial.begin(115200); //打开串口
mympu.initialize(); //初始化mpu6050
}
void loop() {
//通过调用该函数，一次性从mpu6050获取6轴数据,ax,ay,az分别是x轴，y轴，z轴加速度;gx,gy,gz分别是角速度
mympu.getMotion6(&ax,&ay,&az,&gx,&gy,&gz);
beta_x=250.0*gx/BetaRatio; //x轴角速度
beta_y=250.0*gy/BetaRatio; //y轴角速度
beta_z=250.0*gz/BetaRatio; //z轴角速度
Serial.print(beta_x);Serial.print(",");
Serial.print(beta_y);Serial.print(",");
Serial.print(beta_z);Serial.println();
delay(1);
}#include "Wire.h" //I2C通讯库
#include "I2Cdev.h" //
#include "MPU6050.h" //mpu6050库
MPU6050 mympu; //定义mympu对象
float pi=3.1415926; //Π的值，用于单位转换
float AcceRatio=16384.0; //加速度计比例系数
float BetaRatio=32768.0; //加速度计比例系数
float GyroRatio=131.0; //陀螺仪比例系数
//定义3个变量，用于存储倾角数据
float angle_x=0.0,angle_y=0.0,angle_z=0.0;
float beta_x=0.0,beta_y=0.0,beta_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(33, 32, 400000); //开启iic通讯，mpu6050的数据传输协议为iic
Serial.begin(115200); //打开串口
mympu.initialize(); //初始化mpu6050
}
void loop() {
float beta_xs[100],angle_yerr[100];
float beta_x_sum=0.0,beta_x_ave=0.0;
float angle_yerr_sigma2=0.0; //噪声方差
for(int i=0;i<100;i++)
{
//通过调用该函数，一次性从mpu6050获取6轴数据,ax,ay,az分别是x轴，y轴，z轴加速度;gx,gy,gz分别是角速度
mympu.getMotion6(&ax,&ay,&az,&gx,&gy,&gz);
beta_x=250.0*gx/BetaRatio; //x轴角速度
beta_xs[i]=beta_x;
Serial.print(beta_x);Serial.print(" ");
beta_x_sum+=beta_x;
delay(10);
}
Serial.println();
beta_x_ave=beta_x_sum/100.0;
for(int i=0;i<100;i++)
angle_yerr_sigma2+=(beta_xs[i]-beta_x_ave)*(beta_xs[i]-beta_x_ave);
angle_yerr_sigma2=angle_yerr_sigma2/100.0;
Serial.println(angle_yerr_sigma2,4);
delay(10000);
}#include "Wire.h" //I2C通讯库
#include "I2Cdev.h" //
#include "MPU6050.h" //mpu6050库
MPU6050 mympu; //定义mympu对象
float pi=3.1415926; //Π的值，用于单位转换
float AcceRatio=16384.0; //加速度计比例系数
float BetaRatio=32768.0; //加速度计比例系数
float GyroRatio=131.0; //陀螺仪比例系数
//定义3个变量，用于存储倾角数据
float angle_x=0.0,angle_y=0.0,angle_z=0.0;
float beta_x=0.0,beta_y=0.0,beta_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(33, 32, 400000); //开启iic通讯，mpu6050的数据传输协议为iic
Serial.begin(115200); //打开串口
mympu.initialize(); //初始化mpu6050
}
void loop() {
float angle_ys[100]; //采集100个倾角angle_y存储到angle_ys数组
float angle_y_sum=0.0,angle_y_ave=0.0; //100个倾角angle_y之和、平均值
float angle_y_sigma2=0.0; //倾角angle_y噪声方差
float beta_xs[100]; //采集100个角速度beta_x存储到beta_xs数组
float beta_x_sum=0.0,beta_x_ave=0.0; //100个角速度beta_x之和、平均值
float beta_x_sigma2=0.0; //角速度beta_x噪声方差
float CovAngleyBetax=0.0;
for(int i=0;i<100;i++)
{
//通过调用该函数，一次性从mpu6050获取6轴数据,ax,ay,az分别是x轴，y轴，z轴加速度;gx,gy,gz分别是角速度
mympu.getMotion6(&ax,&ay,&az,&gx,&gy,&gz);
accy=ay/AcceRatio; //y轴加速度
accz=az/AcceRatio; //z轴加速度
angle_y=(atan(accy/accz)*180/pi); //计算倾角angle_y
angle_ys[i]=angle_y;
angle_y_sum+=angle_y; //累加angle_y用于求取平均值
beta_x=250.0*gx/BetaRatio; //x轴角速度
beta_xs[i]=beta_x; //累加beta_x用于求取平均值
beta_x_sum+=beta_x;
delay(10);
}
Serial.println();
angle_y_ave=angle_y_sum/100.0; //100个倾角平均值angle_y_ave
beta_x_ave=beta_x_sum/100.0; //100个角速度平均值beta_x_ave
//计算100个倾角angle_y方差
for(int i=0;i<100;i++)
angle_y_sigma2+=(angle_ys[i]-angle_y_ave)*(angle_ys[i]-angle_y_ave);
angle_y_sigma2=angle_y_sigma2/100.0;
Serial.println(angle_y_sigma2,6);
//计算100个角速度beta_x方差
for(int i=0;i<100;i++)
beta_x_sigma2+=(beta_xs[i]-beta_x_ave)*(beta_xs[i]-beta_x_ave);
beta_x_sigma2=beta_x_sigma2/100.0;
Serial.println(beta_x_sigma2,6);
//计算100个倾角angle_y和100个角速度beta_x协方差
for(int i=0;i<100;i++)
CovAngleyBetax+=(angle_ys[i]-angle_y_ave)*(beta_xs[i]-beta_x_ave);
CovAngleyBetax=CovAngleyBetax/100.0;
Serial.println(CovAngleyBetax,6);
delay(10000);
}可以在下方看到，打印出来了这些数值，方差比协方差大几十倍，至少都差了一个数量级。
非常小可以忽略，这也印证了非相关量的协方差为零的数学性质。
我们的R矩阵就是噪声协方差矩阵，只需要把测得的这些数值填进去就可以了。
既然R矩阵已经有了，现在我们把Q矩阵也求出来。
计算Pk的先验矩阵
硬件使用Baize_Servo1
#include "Wire.h"
#include "I2Cdev.h"
#include "MPU6050.h"
MPU6050 accelgyro;
unsigned long now, lastTime = 0;
float dt; //微分时间
int16_t ax, ay, az, gx, gy, gz; //加速度计陀螺仪原始数据
float aax=0, aay=0,aaz=0, agx=0, agy=0, agz=0; //角度变量
long axo = 0, ayo = 0, azo = 0; //加速度计偏移量
long gxo = 0, gyo = 0, gzo = 0; //陀螺仪偏移量
float pi = 3.1415926;
float AcceRatio = 16384.0; //加速度计比例系数
float GyroRatio = 131.0; //陀螺仪比例系数
uint8_t n_sample = 8; //加速度计滤波算法采样个数
float aaxs[8] = {0}, aays[8] = {0}, aazs[8] = {0}; //x,y轴采样队列
long aax_sum, aay_sum,aaz_sum; //x,y轴采样和
float a_x[10]={0}, a_y[10]={0},a_z[10]={0} ,g_x[10]={0} ,g_y[10]={0},g_z[10]={0}; //加速度计协方差计算队列
float Px=1, Rx, Kx, Sx, Vx, Qx; //x轴卡尔曼变量
float Py=1, Ry, Ky, Sy, Vy, Qy; //y轴卡尔曼变量
float Pz=1, Rz, Kz, Sz, Vz, Qz; //z轴卡尔曼变量
void setup()
{
Wire.begin();
Serial.begin(115200);
accelgyro.initialize(); //初始化
unsigned short times = 200; //采样次数
for(int i=0;i<times;i++)
{
accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz); //读取六轴原始数值
axo += ax; ayo += ay; azo += az; //采样和
gxo += gx; gyo += gy; gzo += gz;
}
axo /= times; ayo /= times; azo /= times; //计算加速度计偏移
gxo /= times; gyo /= times; gzo /= times; //计算陀螺仪偏移
}
void loop()
{
unsigned long now = millis(); //当前时间(ms)
dt = (now - lastTime) / 1000.0; //微分时间(s)
lastTime = now; //上一次采样时间(ms)
accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz); //读取六轴原始数值
float accx = ax / AcceRatio; //x轴加速度
float accy = ay / AcceRatio; //y轴加速度
float accz = az / AcceRatio; //z轴加速度
aax = atan(accy / accz) * (-180) / pi; //y轴对于z轴的夹角
aay = atan(accx / accz) * 180 / pi; //x轴对于z轴的夹角
aaz = atan(accz / accy) * 180 / pi; //z轴对于y轴的夹角
Serial.print(aax);Serial.print(",");
Serial.print(aay);Serial.print(",");
Serial.print(aaz);Serial.print(",");
aax_sum = 0; // 对于加速度计原始数据的滑动加权滤波算法
aay_sum = 0;
aaz_sum = 0;
for(int i=1;i<n_sample;i++)
{
aaxs[i-1] = aaxs[i];
aax_sum += aaxs[i] * i;
aays[i-1] = aays[i];
aay_sum += aays[i] * i;
aazs[i-1] = aazs[i];
aaz_sum += aazs[i] * i;
}
aaxs[n_sample-1] = aax;
aax_sum += aax * n_sample;
aax = (aax_sum / (11*n_sample/2.0)) * 9 / 7.0; //角度调幅至0-90°
aays[n_sample-1] = aay; //此处应用实验法取得合适的系数
aay_sum += aay * n_sample; //本例系数为9/7
aay = (aay_sum / (11*n_sample/2.0)) * 9 / 7.0;
aazs[n_sample-1] = aaz;
aaz_sum += aaz * n_sample;
aaz = (aaz_sum / (11*n_sample/2.0)) * 9 / 7.0;
float gyrox = - (gx-gxo) / GyroRatio * dt; //x轴角速度
float gyroy = - (gy-gyo) / GyroRatio * dt; //y轴角速度
float gyroz = - (gz-gzo) / GyroRatio * dt; //z轴角速度
agx += gyrox; //x轴角速度积分
agy += gyroy; //x轴角速度积分
agz += gyroz;
/* kalman start */
Sx = 0; Rx = 0;
Sy = 0; Ry = 0;
Sz = 0; Rz = 0;
for(int i=1;i<10;i++)
{ //测量值平均值运算
a_x[i-1] = a_x[i]; //即加速度平均值
Sx += a_x[i];
a_y[i-1] = a_y[i];
Sy += a_y[i];
a_z[i-1] = a_z[i];
Sz += a_z[i];
}
a_x[9] = aax;
Sx += aax;
Sx /= 10; //x轴加速度平均值
a_y[9] = aay;
Sy += aay;
Sy /= 10; //y轴加速度平均值
a_z[9] = aaz;
Sz += aaz;
Sz /= 10;
for(int i=0;i<10;i++)
{
Rx += sq(a_x[i] - Sx);
Ry += sq(a_y[i] - Sy);
Rz += sq(a_z[i] - Sz);
}
Rx = Rx / 9; //得到方差
Ry = Ry / 9;
Rz = Rz / 9;
Px = Px + 0.0025; // 0.0025在下面有说明...
Kx = Px / (Px + Rx); //计算卡尔曼增益
agx = agx + Kx * (aax - agx); //陀螺仪角度与加速度计速度叠加
Px = (1 - Kx) * Px; //更新p值
Py = Py + 0.0025;
Ky = Py / (Py + Ry);
agy = agy + Ky * (aay - agy);
Py = (1 - Ky) * Py;
Pz = Pz + 0.0025;
Kz = Pz / (Pz + Rz);
agz = agz + Kz * (aaz - agz);
Pz = (1 - Kz) * Pz;
/* kalman end */
Serial.print(agx);Serial.print(",");
Serial.print(agy);Serial.print(",");
Serial.println(agz);//Serial.println();
}
俯仰角 横滚角
偏航角 三轴欧拉角