# not understanding kalman filtering code!?

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

This software may be distributed and modified under the terms of the GNU
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.
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**
** if (i2cData[0] != 0x68) { // Read “WHO_AM_I” register**
** while (1);**
** }**
** delay(100); // Wait for sensor to stabilize**
__ /* Set kalman and gyro starting angle /__
*
** 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 http://en.wikipedia.org/wiki/Atan2**
** // 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 /__
*
** 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 http://en.wikipedia.org/wiki/Atan2**
** // 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);**
}

OK so you are young and you don't read instructions but you need to in order not to annoy people here.

How to use this forum
Because your post is breaking the rules. Did you put all those smiles in your code?

rying to learn the basics of kalman filtering

Why do you think this is going to help you do what you want? It is not magic that will suddenly give you data from noise.

Smiley is what you get when you don’t use code-tags.

``````  /* Set kalman and gyro starting angle */