这学期做一个project，测量骑自行车时所耗功率，其中测量角速度和角位移部分应用Arduino+MPU6050，看到论坛中有一些帖子，但都不太全，测量内容精度也有很大提高空间。首先声明，这些code基本不是本人所写，源程序下载地址：https://github.com/TKJElectronics/KalmanFilter，本人只是稍作修改。希望这些内容对做这方面研究的同学有所帮助。
code主要包括三个文件，主程序MPU6050.ino， I2C.ino 以及Kalman滤波程序 Kalman.h。另附serialChart的scc文件实时显示结果。关于Arduino，MPU6050和serialChart的基本介绍本论坛都可以查到，故不再赘述。
MPU6050.ino
#include <Wire.h>
#include "Kalman.h"
Kalman kalmanX; // Create the Kalman instances
Kalman kalmanY;
/* IMU Data */
int16_t accX, accY, accZ;
int16_t tempRaw;
int16_t gyroX, gyroY, gyroZ;
int16_t gyroXoffset = -139;
int16_t gyroYoffset = -74;
int16_t gyroZoffset = -111; // this value is not accurate at all
double accXangle, accYangle; // Angle calculate using the accelerometer
double temp; // Temperature
double gyroXangle, gyroYangle; // Angle calculate using the gyro
double compAngleX, compAngleY; // Calculate the angle using a complementary filter
double kalAngleX, kalAngleY; // Calculate the angle using a Kalman filter
uint32_t timer;
uint8_t i2cData[14]; // Buffer for I2C data
void setup() {
Serial.begin(115200);
Wire.begin();
i2cData[0] = 7; // Set the sample rate to 1000Hz - 8kHz/(7+1) = 1000Hz
i2cData[1] = 0x00; // Disable FSYNC and set 260 Hz Acc filtering, 256 Hz Gyro filtering, 8 KHz sampling
i2cData[2] = 0x00; // Set Gyro Full Scale Range to ±250deg/s
i2cData[3] = 0x00; // Set Accelerometer Full Scale Range to ±2g
while(i2cWrite(0x19,i2cData,4,false)); // Write to all four registers at once
while(i2cWrite(0x6B,0x01,true)); // PLL with X axis gyroscope reference and disable sleep mode
while(i2cRead(0x75,i2cData,1));
if(i2cData[0] != 0x68) { // Read "WHO_AM_I" register
Serial.print(F("Error reading sensor"));
while(1);
}
delay(100); // Wait for sensor to stabilize
/* Set kalman and gyro starting angle */
while(i2cRead(0x3B,i2cData,6));
accX = ((i2cData[0] << 8) | i2cData[1]);
accY = ((i2cData[2] << 8) | i2cData[3]);
accZ = ((i2cData[4] << 8) | i2cData[5]);
// atan2 outputs the value of -π to π (radians) - see http://en.wikipedia.org/wiki/Atan2
// We then convert it to 0 to 2π and then from radians to degrees
accYangle = (atan2(accX,accZ)+PI)*RAD_TO_DEG;
accXangle = (atan2(accY,accZ)+PI)*RAD_TO_DEG;
kalmanX.setAngle(accXangle); // Set starting angle
kalmanY.setAngle(accYangle);
gyroXangle = accXangle;
gyroYangle = accYangle;
compAngleX = accXangle;
compAngleY = accYangle;
timer = micros();
}
void loop() {
/* Update all the values */
while(i2cRead(0x3B,i2cData,14));
accX = ((i2cData[0] << 8) | i2cData[1]);
accY = ((i2cData[2] << 8) | i2cData[3]);
accZ = ((i2cData[4] << 8) | i2cData[5]);
tempRaw = ((i2cData[6] << 8) | i2cData[7]);
gyroX = ((i2cData[8] << 8) | i2cData[9])-gyroXoffset;
gyroY = ((i2cData[10] << 8) | i2cData[11])-gyroYoffset;
gyroZ = ((i2cData[12] << 8) | i2cData[13])-gyroYoffset;
// atan2 outputs the value of -π to π (radians) - see http://en.wikipedia.org/wiki/Atan2
// We then convert it to 0 to 2π and then from radians to degrees
// Note: if we only consider the angle, we don't need transform the raw acceleration to its physical meaning
// The formulation transforming to its physical meaning (Unit: g): accX = accX/16384.00;
accXangle = (atan2(accY,accZ)+PI)*RAD_TO_DEG;
accYangle = (atan2(accX,accZ)+PI)*RAD_TO_DEG;
double gyroXrate = (double)gyroX/131.0; // circular velocity in X direction
double gyroYrate = -((double)gyroY/131.0); // circular velocity in Y direction
gyroXangle += gyroXrate*((double)(micros()-timer)/1000000); // Calculate gyro angle without any filter
gyroYangle += gyroYrate*((double)(micros()-timer)/1000000);
//gyroXangle += kalmanX.getRate()*((double)(micros()-timer)/1000000); // Calculate gyro angle using the unbiased rate
//gyroYangle += kalmanY.getRate()*((double)(micros()-timer)/1000000);
compAngleX = (0.93*(compAngleX+(gyroXrate*(double)(micros()-timer)/1000000)))+(0.07*accXangle); // Calculate the angle using a Complimentary filter
compAngleY = (0.93*(compAngleY+(gyroYrate*(double)(micros()-timer)/1000000)))+(0.07*accYangle);
kalAngleX = kalmanX.getAngle(accXangle, gyroXrate, (double)(micros()-timer)/1000000); // Calculate the angle using a Kalman filter
kalAngleY = kalmanY.getAngle(accYangle, gyroYrate, (double)(micros()-timer)/1000000);
timer = micros();
temp = ((double)tempRaw + 12412.0) / 340.0;
/* Print Data */
/*
Serial.print(accX);Serial.print(",");
Serial.print(accY);Serial.print(",");
Serial.print(accZ);Serial.print(",");
*/
//Serial.print(gyroX);Serial.print(",");
//Serial.print(gyroY);Serial.print(",");
//Serial.print(gyroZ);Serial.print(",");
Serial.print(gyroXrate);Serial.print(",");
Serial.print(gyroYrate); Serial.print(",");
Serial.print(accXangle);Serial.print(",");
Serial.print(gyroXangle);Serial.print(",");
//Serial.print(compAngleX);Serial.print(",");
Serial.print(kalAngleX);Serial.print(",");
//Serial.print(",");
Serial.print(accYangle);Serial.print(",");
Serial.print(gyroYangle);Serial.print(",");
//Serial.print(compAngleY); Serial.print(",");
Serial.print(kalAngleY); //Serial.print(",");
//Serial.print(temp);Serial.print(",");
Serial.print("\r\n");
delay(1);
}
I2C.ino:
#include <Wire.h>
const uint8_t IMUAddress = 0x68; // AD0 is logic low on the PCB
const uint16_t I2C_TIMEOUT = 1000; // Used to check for errors in I2C communication
uint8_t i2cWrite(uint8_t registerAddress, uint8_t data, bool sendStop) {
return i2cWrite(registerAddress,&data,1,sendStop); // Returns 0 on success
}
uint8_t i2cWrite(uint8_t registerAddress, uint8_t* data, uint8_t length, bool sendStop) {
Wire.beginTransmission(IMUAddress);
Wire.write(registerAddress);
Wire.write(data, length);
return Wire.endTransmission(sendStop); // Returns 0 on success
}
uint8_t i2cRead(uint8_t registerAddress, uint8_t* data, uint8_t nbytes) {
uint32_t timeOutTimer;
Wire.beginTransmission(IMUAddress);
Wire.write(registerAddress);
if(Wire.endTransmission(false)) // Don't release the bus
return 1; // Error in communication
Wire.requestFrom(IMUAddress, nbytes,(uint8_t)true); // Send a repeated start and then release the bus after reading
for(uint8_t i = 0; i < nbytes; i++) {
if(Wire.available())
data = Wire.read();
else {
timeOutTimer = micros();
while(((micros() - timeOutTimer) < I2C_TIMEOUT) && !Wire.available());
if(Wire.available())
data = Wire.read();
else
return 2; // Error in communication
}
}
return 0; // Success
}
Kalman.h
/* Copyright (C) 2012 Kristian Lauszus, TKJ Electronics. All rights reserved.
This software may be distributed and modified under the terms of the GNU
General Public License version 2 (GPL2) as published by the Free Software
Foundation and appearing in the file GPL2.TXT included in the packaging of
this file. Please note that GPL2 Section 2 requires that all works based
on this software must also be made publicly available under the terms of
the GPL2 ("Copyleft").
Contact information
-------------------
Kristian Lauszus, TKJ Electronics
Web : http://www.tkjelectronics.com
e-mail : [email protected]
*/
#ifndef _Kalman_h
#define _Kalman_h
class Kalman {
public:
Kalman() {
/* We will set the varibles like so, these can also be tuned by the user */
Q_angle = 0.001;
Q_bias = 0.003;
R_measure = 0.03;
bias = 0; // Reset bias
P[0][0] = 0; // Since we assume tha the bias is 0 and we know the starting angle (use setAngle), the error covariance matrix is set like so - see: http://en.wikipedia.org/wiki/Kalman_filter#Example_application.2C_technical
P[0][1] = 0;
P[1][0] = 0;
P[1][1] = 0;
};
// The angle should be in degrees and the rate should be in degrees per second and the delta time in seconds
double getAngle(double newAngle, double newRate, double dt) {
// KasBot V2 - Kalman filter module - http://www.x-firm.com/?page_id=145
// Modified by Kristian Lauszus
// See my blog post for more information: http://blog.tkjelectronics.dk/2012/09/a-practical-approach-to-kalman-filter-and-how-to-implement-it
// Discrete Kalman filter time update equations - Time Update ("Predict")
// Update xhat - Project the state ahead
/* Step 1 */
rate = newRate - bias;
angle += dt * rate;
// Update estimation error covariance - Project the error covariance ahead
/* Step 2 */
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;
// Discrete Kalman filter measurement update equations - Measurement Update ("Correct")
// Calculate Kalman gain - Compute the Kalman gain
/* Step 4 */
S = P[0][0] + R_measure;
/* Step 5 */
K[0] = P[0][0] / S;
K[1] = P[1][0] / S;
// Calculate angle and bias - Update estimate with measurement zk (newAngle)
/* Step 3 */
y = newAngle - angle;
/* Step 6 */
angle += K[0] * y;
bias += K[1] * y;
// Calculate estimation error covariance - Update the error covariance
/* Step 7 */
P[0][0] -= K[0] * P[0][0];
P[0][1] -= K[0] * P[0][1];
P[1][0] -= K[1] * P[0][0];
P[1][1] -= K[1] * P[0][1];
return angle;
};
void setAngle(double newAngle) { angle = newAngle; }; // Used to set angle, this should be set as the starting angle
double getRate() { return rate; }; // Return the unbiased rate
/* These are used to tune the Kalman filter */
void setQangle(double newQ_angle) { Q_angle = newQ_angle; };
void setQbias(double newQ_bias) { Q_bias = newQ_bias; };
void setRmeasure(double newR_measure) { R_measure = newR_measure; };
private:
/* Kalman filter variables */
double Q_angle; // Process noise variance for the accelerometer
double Q_bias; // Process noise variance for the gyro bias
double R_measure; // Measurement noise variance - this is actually the variance of the measurement noise
double angle; // The angle calculated by the Kalman filter - part of the 2x1 state matrix
double bias; // The gyro bias calculated by the Kalman filter - part of the 2x1 state matrix
double rate; // Unbiased rate calculated from the rate and the calculated bias - you have to call getAngle to update the rate
double P[2][2]; // Error covariance matrix - This is a 2x2 matrix
double K[2]; // Kalman gain - This is a 2x1 matrix
double y; // Angle difference - 1x1 matrix
double S; // Estimate error - 1x1 matrix
};
#endif
imu_arduino.scc
[_setup_]
port=COM3
baudrate=115200
width=1500
height=251
background_color = white
grid_h_origin = 125
grid_h_step = 50
grid_h_color = #EEE
grid_h_origin_color = #CCC
grid_v_origin = 0
grid_v_step = 200
grid_v_color = #EEE
grid_v_origin_color = transparent
[_default_]
min=-1
max=1
[gyroYrate]
color=transparent
min = -10
max = 10
[gyroXrate]
color=blue
min = -1000
max = 1000
[accXangle]
color=transparent
min=0
max=360
[accYangle]
color=transparent
min=0
max=360
[gyroXangle]
color=transparent
min=0
max=360
[gyroYangle]
color=green
min=0
max=360
[kalAngleX]
color=transparent
min=0
max=360
[kalAngleY]
color=red
min=0
max=360
|
发表于 2013-5-6 08:18:25
折磨人啊
