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

/* 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 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 /__
** 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 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.

Please read this:-
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 */
  while (i2cRead(0x3B, i2cData, 6));
  accX = (i2cData[0] << 8) | i2cData[1];
  accY = (i2cData[2] << 8) | i2cData[3];
  accZ = (i2cData[4] << 8) | i2cData[5];

There is no Kalman filter in the bit you posted. The actual filter code is in a library elsewhere, but it is wrong and does not work well. See this page for a description of its failings and a working alternative in the Scilab programming language here.

The purpose of code you posted seems to be to compare pitch and roll angles generated by the (incorrectly programmed) Kalman filter and by complementary filtering, a different approach. Overall, the entire effort seems useless and is not recommended for study.

If you want to understand Kalman filters, you will need a college-level course in linear algebra (matrix theory and operations). However, if you are willing to work your way through a good textbook, I recommend "Optimal State Estimation" by Dan Simon.