正在更新中。。。
这篇文章要跟大家一起完全搞明白卡尔曼滤波，连一个标点符号也不放过，完完全全理解明白。
引例及思路
推导卡尔曼增益K?
?
?计算Pk的先验矩阵
?
什么是过程噪声？
Q是过程噪声协方差矩阵，那什么是过程噪声？
比如我有一个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);
}
