-------這篇文章就作爲放棄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有個圖