卡尔曼滤波融合库函数+Arduino实例

-------这篇文章就作为放弃ACM比赛转行到电子设计大赛的开始吧，ACM比赛真的太需要时间了，准确的说对于我这样的菜鸟而言太浪费时间了，但是话说回来两年时间从中真心收获了很多

        我是不理解卡尔曼滤波的原理啊，但是用这个库函数做个平衡车是绝对没问题 ，所以不理解没太大问题，只要知道它是用来融合加速度计 和 陀螺仪测定角度的。这个角度相对单纯求得的角度会更加精确，既然我弄不明白滤波的原理，下面我会特别详细的说明一下此库函数用到的变量，毕竟有很多人还是想弄明白的。

Q_angle：相对于加速度计的噪音协方差。    Q_bias：相对于陀螺仪的噪音协方差。  

R_measure：测量噪声协方差（有的版本是R_angle说是ACC的协方差）。  

***********************************以上私有变量使用者可以根据具体情况稍作修改，并且库提供set的函数***********************************

angle：最终要算得的角度。  bias：最终算得的陀螺零偏。  rate：最终算得的角速度。 P[2][2]：协方差误差矩阵。

************************************************************再说一下getAngle函数的参数*******************************************************

newAngle:这是去除了LBS偏差的角度单位是度。   newRate:去除了LBS偏差的角速度单位是度/秒。 dt：增量时间采样频率

**********************************************Kalman.h文件 当时找到没法复制 我就手打 所以忘记了作者***********************************

#ifndef _Kalman_h_
#define _Kalman_h_

class Kalman {
public:
Kalman();

float getAngle(float newAngle, float newRate, float dt);

void setAngle(float angle);
float getRate();

void setQangle(float Q_angle);
void setQbias(float Q_bias);
void setRmeasure(float R_measure);

float getQangle();
float getQbias();
float getRmeasure();

private:
float Q_angle;
float Q_bias;
float R_measure;

float angle;
float bias;
float rate;

float P[2][2];
};

#endif

*******************************************Kalman.cpp文件 当时找到没法复制 我就手打 所以忘记了作者**********************************

#include "Kalman.h"

Kalman::Kalman() {
Q_angle = 0.001f;
Q_bias = 0.003f;
R_measure = 0.03f;

angle = 0.0f;
bias = 0.0f;

P[0][0] = 0.0f;
P[0][1] = 0.0f;
P[1][0] = 0.0f;
P[1][1] = 0.0f;
};

float Kalman::getAngle(float newAngle, float newRate, float dt) {
rate = newRate - bias;
angle += dt * rate;
P[0][0] += dt * (dt*P[1][1] - P[0][1] - P[1][0] + Q_angle);
P[0][1] -= dt * P[1][1];
P[1][0] -= dt * P[1][1];
P[1][1] += Q_bias * dt;

float S = P[0][0] + R_measure;
float K[2];
K[0] = P[0][0] / S;
K[1] = P[1][0] / S;

float y = newAngle - angle;
angle += K[0] * y;
bias += K[1] * y;

float P00_temp = P[0][0];
float P01_temp = P[0][1];

P[0][0] -= K[0] * P00_temp;
P[0][1] -= K[0] * P01_temp;
P[1][0] -= K[1] * P00_temp;
P[1][1] -= K[1] * P01_temp;

return angle;
};

void Kalman::setAngle(float angle) { this->angle = angle; };
float Kalman::getRate() { return this->rate; };

void Kalman::setQangle(float Q_angle) { this->Q_angle = Q_angle; };
void Kalman::setQbias(float Q_bias) { this->Q_bias = Q_bias; };
void Kalman::setRmeasure(float R_measure) { this->R_measure = R_measure; };

float Kalman::getQangle() { return this->Q_angle; };
float Kalman::getQbias() { return this->Q_bias; };
float Kalman::getRmeasure() { return this->R_measure; };


再附上一个Arduino示例程序用来观察自己mpu6050波形情况（因为每个人的mpu6050不一样可将各协方差稍作调整）

/*
title  : Kalman fusion angle calculation
date   : 08/08/2015
author : houwei
*/

#include "Wire.h"
#include "I2Cdev.h"
#include "MPU6050.h"
#include "Kalman.h"

#define AX_ZERO -1181 //加速度计的0偏修正值
#define GX_ZERO -176.85 //陀螺仪的0偏修正

Kalman angle_feng;
MPU6050 accelgyro;

int16_t ax, ay, az;
int16_t gx, gy, gz;
double total_angle = 0;

bool blinkState = false;

void setup() {
Wire.begin();

Serial.begin(38400);

Serial.println("Initializing I2C devices...");
accelgyro.initialize();

Serial.println("Testing device connections...");
Serial.println(accelgyro.testConnection() ? "MPU6050 connection successful" : "MPU6050 connection failed");

}
float Angle = 0.0; //卡尔曼融合最终角度
unsigned long pretime = 0.0;//相当于执行的起始时间
void loop()
{
double ax_angle = 0.0; //加速度计算得的角度
double gx_angle = 0.0; //微分的每次角速度算得的角度
double totgx_angle = 0.0; //总的角速度算得的角度
unsigned long time = 0; //每执行一次loop所用时间
unsigned long midtime = 0; //相当于执行的结束时间
float gyro = 0.0, dt = 0.0;
if (pretime == 0) pretime = millis();
midtime = millis();
accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
ax = ax - AX_ZERO;
/*
initialize（）中加速的范围设置为2g
分辨率：16384 LSB/g
sin(jiaodu) = k(xishu = 0.92) * 3.14 * (jiaodu) / 180
= （jiasudu） / 16384 (jiasudu/16384 加速度
对应范围的实际值）
*/
ax_angle = ax/263;
/*
initialize（）中加速的范围设置为250度/s
分辨率：131 LSB/S
gx_angle = ((gy/131)*dt)/1000
totgx_angle += gx_angle
*/
gy -= GX_ZERO;
time = midtime - pretime;

gyro = gy/131.0;
gx_angle = gyro * time;
gx_angle /= 1000.0;
total_angle += gx_angle;

dt = time / 1000.0;
Angle = angle_feng.getAngle(ax_angle, gyro, dt);

delay(1000);
Serial.print(ax_angle);Serial.print(", ");
Serial.print(total_angle); Serial.print(", ");
Serial.print(Angle);

pretime = midtime;
}

上面有两个initialize()设置问题一个是加速度计-2g~+2g，对应16384 LSB/g  另一个陀螺仪-250度/s~+250度/s 对应131LSB/s有个图