+arduino Example of Kalman filter Fusion library function

-------This article as a waiver of ACM competition to the beginning of the Electronic Design Competition, ACM competition really need time, accurate say for me such a rookie is too waste of time, but then two years time from the real harvest a lot of

I do not understand the principle of Kalman filter, Ah, but with this library function to do a balanced car is absolutely no problem, so do not understand not too much problem, as long as it is used to fuse accelerometer and gyroscope measurement angle. This angle is relatively simple to obtain the angle will be more accurate, since I do not understand the principle of filtering, below I will specifically explain the library function used variables, after all, many people still want to understand.

Q_angle: Noise covariance relative to the accelerometer. Q_bias: The noise covariance relative to the gyroscope.

R_measure: Measure noise covariance (some versions are r_angle that are ACC's covariance).

The above-mentioned private variables can be modified slightly depending on the situation, and the library provides a set function ***********************************

angle: The final angle to calculate.  Bias: The final calculated gyro 0 deviation. Rate: The final computed angular velocity. p[2][2]: Covariance error matrix.

Again, the parameters of the Getangle function ********************************** *********************

Newangle: This is the degree to which the angle unit of the lbs deviation is removed. Newrate: The angular velocity unit for which the lbs deviation is removed is degrees per second. DT: Incremental Time sampling frequency

Kalman.h file was found unable to copy my personal overdraft, so I forgot the author ***********************************

#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 ();p rivate:    float q_angle;    float Q_bias;    float r_measure;    float angle;    float bias;    float rate;    float p[2][2];}; #endif

Kalman.cpp file was found unable to copy my personal overdraft, so I forgot the author **********************************

#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-&Gt 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;};

Attach an Arduino sample program to watch your mpu6050 waveform situation ****************************************** **

/*title:kalman Fusion angle calculationdate:08/08/2015author:houwei*/#include "Wire.h" #include "I2Cdev.h" #include "MPU6050.h" #include "Kalman.h" #define AX_ZERO-1181//Accelerometer 0 Correction Value # define gx_zero-176.85//Gyroscope 0-offset modified Kalman Angle_feng; MPU6050 accelgyro;int16_t ax, ay, az;int16_t GX, GY, gz;double total_angle = 0;bool blinkstate = false;void Setup () {W    Ire.begin ();    Serial.begin (38400);    Serial.println ("Initializing i²c devices ...");    Accelgyro.initialize ();    SERIAL.PRINTLN ("Testing device connections ..."); Serial.println (Accelgyro.testconnection ()?    "MPU6050 Connection Successful": "MPU6050 Connection Failed"); Pinmode (Led_pin, OUTPUT);} float Angle = 0.0;  Kalman Fusion Final Angle unsigned long pretime = 0.0;//equivalent to the start time of execution void Loop () {Double ax_angle = 0.0;//acceleration calculated angle Double Gx_angle = 0.0; The angular velocity of the differential is computed at the angle of double totgx_angle = 0.0; The total angular velocity of the computed angle unsigned long time = 0; Time per loop executed unsigned long midtime = 0;    Equivalent to the end time of the execution of 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 () The accelerated range is set to 2g resolution: 16384 lsb/g sin (jiaodu) = k (Xishu = 0.92) * 3.14 * (JIAODU)/180 =    (Jiasudu)/16384 (actual value of jiasudu/16384 acceleration corresponding range) */ax_angle = ax/263;  /* Initialize () The accelerated range is set to 250 degrees/s resolution: 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;}

There are two initialize () setting problems one is the accelerometer -2g~+2g, corresponding to 16384 lsb/g another gyroscope-250 degrees/s~+250 degrees/s corresponds to 131lsb/s there is a diagram



Copyright NOTICE: This article for Bo Master original article, without Bo Master permission not reproduced.

+arduino Example of Kalman filter Fusion library function