Kalman Filter with MPU6050

Hi
I am using the MPU6050 to record data from the serial monitor and I have been researching how to get more accurate readings. And I have done lots of research and found the Kalman Filter library by TKJ Electronics. I uploaded the code from the example sketch and I verify it and it comes up with the Error Message Error compiling for board Arduino Nano.

Arduino: 1.8.13 (Windows 10), Board: "Arduino Nano, ATmega328P (Old Bootloader)"





















C:\Users\insca\AppData\Local\Temp\ccRe1KEu.ltrans0.ltrans.o: In function `global constructors keyed to 65535_0_MPU6050.ino.cpp.o.1975':

<artificial>:(.text.startup+0x74): undefined reference to `Kalman::Kalman()'

<artificial>:(.text.startup+0x7c): undefined reference to `Kalman::Kalman()'

C:\Users\insca\AppData\Local\Temp\ccRe1KEu.ltrans0.ltrans.o: In function `setup':

C:\Users\insca\AppData\Local\Temp\arduino_modified_sketch_317714/MPU6050.ino:80: undefined reference to `Kalman::setAngle(float)'

C:\Users\insca\AppData\Local\Temp\arduino_modified_sketch_317714/MPU6050.ino:81: undefined reference to `Kalman::setAngle(float)'

C:\Users\insca\AppData\Local\Temp\ccRe1KEu.ltrans0.ltrans.o: In function `loop':

C:\Users\insca\AppData\Local\Temp\arduino_modified_sketch_317714/MPU6050.ino:121: undefined reference to `Kalman::setAngle(float)'

C:\Users\insca\AppData\Local\Temp\arduino_modified_sketch_317714/MPU6050.ino:130: undefined reference to `Kalman::getAngle(float, float, float)'

C:\Users\insca\AppData\Local\Temp\arduino_modified_sketch_317714/MPU6050.ino:126: undefined reference to `Kalman::getAngle(float, float, float)'

collect2.exe: error: ld returned 1 exit status

exit status 1

Error compiling for board Arduino Nano.



This report would have more information with
"Show verbose output during compilation"
option enabled in File -> Preferences.

Code I am using

/* 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
*/
 #include "Wire.h"
#include "Kalman.h" // Source: https://github.com/TKJElectronics/KalmanFilter

#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 = (int16_t)((i2cData[0] << 8) | i2cData[1]);
  accY = (int16_t)((i2cData[2] << 8) | i2cData[3]);
  accZ = (int16_t)((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 = (int16_t)((i2cData[0] << 8) | i2cData[1]);
  accY = (int16_t)((i2cData[2] << 8) | i2cData[3]);
  accZ = (int16_t)((i2cData[4] << 8) | i2cData[5]);
  tempRaw = (int16_t)((i2cData[6] << 8) | i2cData[7]);
  gyroX = (int16_t)((i2cData[8] << 8) | i2cData[9]);
  gyroY = (int16_t)((i2cData[10] << 8) | i2cData[11]);
  gyroZ = (int16_t)((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(100);
}

And also using the I2C file with it.
Thanks

Try to compile for some common controller (Uno...). Your board may not be supported?

Same outcome

Arduino: 1.8.13 (Windows 10), Board: "Arduino Uno"





















C:\Users\insca\AppData\Local\Temp\ccuRcMQH.ltrans0.ltrans.o: In function `global constructors keyed to 65535_0_MPU6050.ino.cpp.o.1975':

<artificial>:(.text.startup+0x74): undefined reference to `Kalman::Kalman()'

<artificial>:(.text.startup+0x7c): undefined reference to `Kalman::Kalman()'

C:\Users\insca\AppData\Local\Temp\ccuRcMQH.ltrans0.ltrans.o: In function `setup':

C:\Users\insca\AppData\Local\Temp\arduino_modified_sketch_508546/MPU6050.ino:80: undefined reference to `Kalman::setAngle(float)'

C:\Users\insca\AppData\Local\Temp\arduino_modified_sketch_508546/MPU6050.ino:81: undefined reference to `Kalman::setAngle(float)'

C:\Users\insca\AppData\Local\Temp\ccuRcMQH.ltrans0.ltrans.o: In function `loop':

C:\Users\insca\AppData\Local\Temp\arduino_modified_sketch_508546/MPU6050.ino:121: undefined reference to `Kalman::setAngle(float)'

C:\Users\insca\AppData\Local\Temp\arduino_modified_sketch_508546/MPU6050.ino:130: undefined reference to `Kalman::getAngle(float, float, float)'

C:\Users\insca\AppData\Local\Temp\arduino_modified_sketch_508546/MPU6050.ino:126: undefined reference to `Kalman::getAngle(float, float, float)'

collect2.exe: error: ld returned 1 exit status

exit status 1

Error compiling for board Arduino Uno.



This report would have more information with
"Show verbose output during compilation"
option enabled in File -> Preferences.

Thanks for the reply

WHAT!!!! It works now I deleted the code and got the new example for the kalman filter library and it works. It must have been my computer or the IDE but it works now.

Thanks
whoo hoo!
:grinning: