极客工坊

 找回密码
 注册

QQ登录

只需一步,快速开始

查看: 35663|回复: 12

Arduino+MPU6050+Kalman filter (all codes)

[复制链接]
发表于 2013-5-6 08:18:25 | 显示全部楼层 |阅读模式
这学期做一个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   :  kristianl@tkjelectronics.com
*/

#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 10:45:17 | 显示全部楼层
楼主能剖析一下这个卡尔曼库吗,比如Q_angle = 0.001; Q_bias = 0.003; R_measure = 0.03;这几个参数如何调整。
回复 支持 反对

使用道具 举报

发表于 2013-5-19 12:18:51 | 显示全部楼层
请问你用的这个代码是能测出位移XYZ各个坐标吗?
回复 支持 反对

使用道具 举报

发表于 2013-7-5 11:08:49 | 显示全部楼层
谢谢楼主的分享,想问楼主一个问题,请问楼主的状态方程和测量方程分别是什么样的?
回复 支持 反对

使用道具 举报

发表于 2014-4-15 16:10:11 | 显示全部楼层
代码挺长的啊
回复 支持 反对

使用道具 举报

发表于 2014-7-16 03:24:25 | 显示全部楼层
今天也在想同样的问题。请问lz施加在飞轮上的力是怎么测量的?
回复 支持 反对

使用道具 举报

发表于 2014-11-28 14:59:21 | 显示全部楼层
怎么编译一直出错?我的做法是把三个文件分别粘贴到一个文件下,可是在运行的时候一直出错,说是找不到h文件,哪里错了吗?
回复 支持 反对

使用道具 举报

发表于 2014-12-5 19:22:13 | 显示全部楼层
这个代码不错,有意思
回复 支持 反对

使用道具 举报

发表于 2014-12-8 17:26:54 | 显示全部楼层
这个代码有开源库,但是,我怎么测出来的卡尔曼滤波还没互补滤波效果好1!!!
做小车滴话PID要怎么调啊,那个大神顺便指教一番
回复 支持 反对

使用道具 举报

发表于 2015-1-1 22:22:53 | 显示全部楼层
长知识,顶一下 !!!
回复 支持 反对

使用道具 举报

发表于 2015-1-6 10:16:02 | 显示全部楼层
折磨人啊
回复 支持 反对

使用道具 举报

发表于 2015-1-13 14:23:43 | 显示全部楼层
楼主东西是好东西,如果能分成4个文件,别人看的就没那么累了。谢谢!~~
回复 支持 反对

使用道具 举报

发表于 2015-1-13 14:40:05 | 显示全部楼层
谢谢分享学习一下
回复 支持 反对

使用道具 举报

您需要登录后才可以回帖 登录 | 注册

本版积分规则

Archiver|联系我们|极客工坊 ( 浙ICP备09023225号 )

GMT+8, 2020-6-3 05:22 , Processed in 0.052828 second(s), 25 queries .

Powered by Discuz! X3.4 Licensed

© 2001-2017 Comsenz Inc.

快速回复 返回顶部 返回列表