Hi,
I'm 16 years old and trying to learn the basics of kalman filtering. My project is to self navigate a GPS car from way point to way point. Because there is a lot of deviation formed from the car, I am trying to use INS with the GPS to tune the cars movements.
I've read some of the basics of the kalman filtering from Wikipedia, but I cannot understand the code below.
Can someone explain it? thanks
/* 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
*/
#include <Wire.h>
#include <Kalman.h> // Source: GitHub - TKJElectronics/KalmanFilter: This is a Kalman filter used to calculate the angle, rate and bias from from the input of an accelerometer/magnetometer and a gyroscope.
#define RESTRICT_PITCH // Comment out to restrict roll to ±90deg instead - please read: http://www.freescale.com/files/sensors/doc/app_note/AN3461.pdf
Kalman kalmanX; // Create the Kalman instances
Kalman kalmanY;
/* IMU Data */
double accX, accY, accZ;
double gyroX, gyroY, gyroZ;
int16_t tempRaw;
double gyroXangle, gyroYangle; // Angle calculate using the gyro only
double compAngleX, compAngleY; // Calculated angle using a complementary filter
double kalAngleX, kalAngleY; // Calculated angle using a Kalman filter
uint32_t timer;
uint8_t i2cData[14]; // Buffer for I2C data
// TODO: Make calibration routine
void setup() {
** Serial.begin(115200);**
** Wire.begin();**
#if ARDUINO >= 157
** Wire.setClock(400000UL); // Set I2C frequency to 400kHz**
#else
** TWBR = ((F_CPU / 400000UL) - 16) / 2; // Set I2C frequency to 400kHz**
#endif
** 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];**
** // Source: http://www.freescale.com/files/sensors/doc/app_note/AN3461.pdf eq. 25 and eq. 26**
** // atan2 outputs the value of -π to π (radians) - see atan2 - Wikipedia**
** // It is then converted from radians to degrees**
#ifdef RESTRICT_PITCH // Eq. 25 and 26
double roll = atan2(accY, accZ) * RAD_TO_DEG;
double pitch = atan(-accX / sqrt(accY * accY + accZ * accZ)) * RAD_TO_DEG;
#else // Eq. 28 and 29
double roll = atan(accY / sqrt(accX * accX + accZ * accZ)) * RAD_TO_DEG;
double pitch = atan2(-accX, accZ) * RAD_TO_DEG;
#endif
** kalmanX.setAngle(roll); // Set starting angle**
** kalmanY.setAngle(pitch);**
** gyroXangle = roll;**
** gyroYangle = pitch;**
** compAngleX = roll;**
** compAngleY = pitch;**
** 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];**
** double dt = (double)(micros() - timer) / 1000000; // Calculate delta time**
** timer = micros();**
** // Source: http://www.freescale.com/files/sensors/doc/app_note/AN3461.pdf eq. 25 and eq. 26**
** // atan2 outputs the value of -π to π (radians) - see atan2 - Wikipedia**
** // It is then converted from radians to degrees**
#ifdef RESTRICT_PITCH // Eq. 25 and 26
double roll = atan2(accY, accZ) * RAD_TO_DEG;
double pitch = atan(-accX / sqrt(accY * accY + accZ * accZ)) * RAD_TO_DEG;
#else // Eq. 28 and 29
double roll = atan(accY / sqrt(accX * accX + accZ * accZ)) * RAD_TO_DEG;
double pitch = atan2(-accX, accZ) * RAD_TO_DEG;
#endif
** double gyroXrate = gyroX / 131.0; // Convert to deg/s**
** double gyroYrate = gyroY / 131.0; // Convert to deg/s**
#ifdef RESTRICT_PITCH
** // This fixes the transition problem when the accelerometer angle jumps between -180 and 180 degrees**
** if ((roll < -90 && kalAngleX > 90) || (roll > 90 && kalAngleX < -90)) {**
** kalmanX.setAngle(roll);**
** compAngleX = roll;**
** kalAngleX = roll;**
** gyroXangle = roll;**
** } else**
** kalAngleX = kalmanX.getAngle(roll, gyroXrate, dt); // Calculate the angle using a Kalman filter**
** if (abs(kalAngleX) > 90)**
** gyroYrate = -gyroYrate; // Invert rate, so it fits the restriced accelerometer reading**
** kalAngleY = kalmanY.getAngle(pitch, gyroYrate, dt);**
#else
** // This fixes the transition problem when the accelerometer angle jumps between -180 and 180 degrees**
** if ((pitch < -90 && kalAngleY > 90) || (pitch > 90 && kalAngleY < -90)) {**
** kalmanY.setAngle(pitch);**
** compAngleY = pitch;**
** kalAngleY = pitch;**
** gyroYangle = pitch;**
** } else**
** kalAngleY = kalmanY.getAngle(pitch, gyroYrate, dt); // Calculate the angle using a Kalman filter**
** if (abs(kalAngleY) > 90)**
** gyroXrate = -gyroXrate; // Invert rate, so it fits the restriced accelerometer reading**
** kalAngleX = kalmanX.getAngle(roll, gyroXrate, dt); // Calculate the angle using a Kalman filter**
#endif
__ gyroXangle += gyroXrate * dt; // Calculate gyro angle without any filter__
__ gyroYangle += gyroYrate * dt;__
__ //gyroXangle += kalmanX.getRate() * dt; // Calculate gyro angle using the unbiased rate__
__ //gyroYangle += kalmanY.getRate() * dt;__
__ compAngleX = 0.93 * (compAngleX + gyroXrate * dt) + 0.07 * roll; // Calculate the angle using a Complimentary filter__
__ compAngleY = 0.93 * (compAngleY + gyroYrate * dt) + 0.07 * pitch;__
** // Reset the gyro angle when it has drifted too much**
** if (gyroXangle < -180 || gyroXangle > 180)**
** gyroXangle = kalAngleX;**
** if (gyroYangle < -180 || gyroYangle > 180)**
** gyroYangle = kalAngleY;**
__ /* Print Data /__
#if 0 // Set to 1 to activate
** Serial.print(accX); Serial.print("\t");*
** Serial.print(accY); Serial.print("\t");**
** Serial.print(accZ); Serial.print("\t");**
** Serial.print(gyroX); Serial.print("\t");**
** Serial.print(gyroY); Serial.print("\t");**
** Serial.print(gyroZ); Serial.print("\t");**
** Serial.print("\t");**
#endif
** Serial.print(roll); Serial.print("\t");**
** Serial.print(gyroXangle); Serial.print("\t");**
** Serial.print(compAngleX); Serial.print("\t");**
** Serial.print(kalAngleX); Serial.print("\t");**
** Serial.print("\t");**
** Serial.print(pitch); Serial.print("\t");**
** Serial.print(gyroYangle); Serial.print("\t");**
** Serial.print(compAngleY); Serial.print("\t");**
** Serial.print(kalAngleY); Serial.print("\t");**
#if 0 // Set to 1 to print the temperature
** Serial.print("\t");**
** double temperature = (double)tempRaw / 340.0 + 36.53;**
** Serial.print(temperature); Serial.print("\t");**
#endif
** Serial.print("\r\n");**
** delay(2);**
}