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[i] = Wire.read();
else {
timeOutTimer = micros();
while(((micros() - timeOutTimer) < I2C_TIMEOUT) && !Wire.available());
if(Wire.available())
data[i] = Wire.read();
else
return 2; // Error in communication
}
}
return 0; // Success
}
/* 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[b] 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
struct Kalman {
/* 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 vector
double bias; // The gyro bias calculated by the Kalman filter - part of the 2x1 state vector
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 vector
double y; // Angle difference
double S; // Estimate error
};
void Init(struct Kalman* klm){
/* We will set the variables like so, these can also be tuned by the user */
klm->Q_angle = 0.001;
klm->Q_bias = 0.003;
klm->R_measure = 0.03;
klm->angle = 0; // Reset the angle
klm->bias = 0; // Reset bias
klm->P[0][0] = 0; // Since we assume that 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
klm->P[0][1] = 0;
klm->P[1][0] = 0;
klm->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(struct Kalman * klm, 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
float P00_temp;
float P01_temp;
// Discrete Kalman filter time update equations - Time Update ("Predict")
// Update xhat - Project the state ahead
/* Step 1 */
klm->rate = newRate - klm->bias;
klm->angle += dt * klm->rate;
// Update estimation error covariance - Project the error covariance ahead
/* Step 2 */
klm->P[0][0] += dt * (dt*klm->P[1][1] - klm->P[0][1] - klm->P[1][0] + klm->Q_angle);
klm->P[0][1] -= dt * klm->P[1][1];
klm->P[1][0] -= dt * klm->P[1][1];
klm->P[1][1] += klm->Q_bias * dt;
// Discrete Kalman filter measurement update equations - Measurement Update ("Correct")
// Calculate Kalman gain - Compute the Kalman gain
/* Step 4 */
klm->S = klm->P[0][0] + klm->R_measure;
/* Step 5 */
klm->K[0] = klm->P[0][0] / klm->S;
klm->K[1] = klm->P[1][0] / klm->S;
// Calculate angle and bias - Update estimate with measurement zk (newAngle)
/* Step 3 */
klm->y = newAngle - klm->angle;
/* Step 6 */
klm->angle += klm->K[0] * klm->y;
klm->bias += klm->K[1] * klm->y;
// Calculate estimation error covariance - Update the error covariance
/* Step 7 */
P00_temp = klm->P[0][0];
P01_temp = klm->P[0][1];
klm->P[0][0] -= klm->K[0] * P00_temp;
klm->P[0][1] -= klm->K[0] * P01_temp;
klm->P[1][0] -= klm->K[1] * P00_temp;
klm->P[1][1] -= klm->K[1] * P01_temp;
return klm->angle;
}
// Used to set angle, this should be set as the starting angle
void setAngle(struct Kalman* klm, double newAngle) { klm->angle = newAngle; }
// Return the unbiased rate
double getRate(struct Kalman* klm) { return klm->rate; }
/* These are used to tune the Kalman filter */
void setQangle(struct Kalman* klm, double newQ_angle) { klm->Q_angle = newQ_angle; }
/* Default value is (0.003f), raise this to follow input more closely, lower this to smooth result of kalman filter */
void setQbias(struct Kalman* klm, double newQ_bias) { klm->Q_bias = newQ_bias; }
void setRmeasure(struct Kalman* klm, double newR_measure) { klm->R_measure = newR_measure; }
double getQangle(struct Kalman* klm) { return klm->Q_angle; }
double getQbias(struct Kalman* klm) { return klm->Q_bias; }
double getRmeasure(struct Kalman* klm) { return klm->R_measure; }
#endif
/* 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[b] 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
Updated by Joe YOON in 2017.12.02
*/
#include <Wire.h>
#include "Kalman.h"
struct kalman Kal_struct_X, Kal_struct_Y; // Create the Kalman instances
/* IMU Data */
int16_t accX, accY, accZ; // 3-axis accelerometer
int16_t tempRaw;
int16_t gyroX, gyroY, gyroZ; // 3-axis gyroscope
double accXangle, accYangle; // Angle calculate using the accelerometer
double temp; // Temperature
double gyroXangle, gyroYangle; // Rate 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() {
Init(&Kal_struct_X); // Initialize Kalman filter for X-axis
Init(&Kal_struct_Y); // Initialize Kalman filter for Y-axis
Serial.begin(9600);
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 +/-250[deg/s]
i2cData[3] = 0x00; // Set Accelerometer Full Scale Range to +/-2[g]
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(-1*accX/sqrt(pow(accY,2) + pow(accZ,2)))*RAD_TO_DEG;
accXangle = atan2(accY/sqrt(pow(accX,2) + pow(accZ,2)))*RAD_TO_DEG;
setAngle(&Kal_struct_X, accXangle); // Set starting angle
setAngle(&Kal_struct_Y, 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]);
gyroY = ((i2cData[10] << 8) | i2cData[11]);
gyroZ = ((i2cData[12] << 8) | i2cData[13]);
// 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(-1*accX/sqrt(pow(accY,2) + pow(accZ,2)))*RAD_TO_DEG;
accXangle = atan2(accY/sqrt(pow(accX,2) + pow(accZ,2)))*RAD_TO_DEG;
double gyroXrate = (double)gyroX/131.0;
double gyroYrate = -((double)gyroY/131.0);
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 = getAngle(&Kal_struct_X, accXangle, gyroXrate, (double)(micros()-timer)/1000000); // Calculate the angle using a Kalman filter
kalAngleY = getAngle(&Kal_struct_Y, accYangle, gyroYrate, (double)(micros()-timer)/1000000);
timer = micros();
temp = ((double)tempRaw + 12412.0) / 340.0;
/* Print Data */
display_formatted_float(accX, 5, 0, 3, false);
display_formatted_float(accY, 5, 0, 3, false);
display_formatted_float(accZ, 5, 0, 3, false);
display_formatted_float(gyroX, 5, 0, 3, false);
display_formatted_float(gyroY, 5, 0, 3, false);
display_formatted_float(gyroZ, 5, 0, 3, false);
Serial.print("\t");
display_formatted_float(accXangle, 5, 2, 3, false);
display_formatted_float(gyroXangle, 5, 2, 3, false);
display_formatted_float(compAngleX, 5, 2, 3, false);
display_formatted_float(kalAngleX, 5, 2, 3, false);
Serial.print("\t");
display_formatted_float(accYangle, 5, 2, 3, false);
display_formatted_float(gyroYangle, 5, 2, 3, false);
display_formatted_float(compAngleY, 5, 2, 3, false);
display_formatted_float(kalAngleY, 5, 2, 3, false);
//Serial.print(temp);Serial.print("\t");
Serial.print("\r\n");
delay(1);
}
void display_formatted_float(double val, int characteristic, int mantissa, int blank, boolean linefeed) {
char outString[16];
int len;
dtostrf(val, characteristic, mantissa, outString);
len = strlen(outString);
for(int i = 0; i < ((characteristic+mantissa+blank)-len); i++) Serial.print(F(" "));
Serial.print(outString);
if(linefeed)
Serial.print(F("\n"));
}
