Hey! Ive tried to combine a code from the TJK's kalman filter library's example. Code compiles without problem but kalman and complementary roll angle outputs "nan" pitch angle is true. Can you help me what causes pitch to be nan?
#include <Wire.h>
#include <Kalman.h>
#include <MPU6050.h>
MPU6050 mpu;
Kalman kalmanX; // Create the Kalman instances
Kalman kalmanY;
/* IMU Data */
int16_t accX, accY, accZ;
int16_t gyroX, gyroY, gyroZ;
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;
void setup() {
Serial.begin(9600);
Wire.begin();
mpu.initialize();
delay(100);
}
void loop() {
mpu.getMotion6(&accX, &accY, &accZ, &gyroX, &gyroY, &gyroZ);
timer = micros();
double dt = (double)(micros() - timer) / 1000000; // Calculate delta time
double roll = atan(accY / sqrt(accX * accX + accZ * accZ)) * RAD_TO_DEG;
double pitch = atan2(-accX, accZ) * RAD_TO_DEG;
double gyroXrate = gyroX / 131.0; // Convert to deg/s
double gyroYrate = gyroY / 131.0; // Convert to deg/s
kalmanY.setAngle(pitch);
compAngleY = pitch;
kalAngleY = pitch;
gyroYangle = pitch;
kalAngleY = kalmanY.getAngle(pitch, gyroYrate, dt); // Calculate the angle using a Kalman filter
kalmanX.setAngle(roll);
compAngleX = roll;
kalAngleX = roll;
gyroXangle = roll;
kalAngleX = kalmanX.getAngle(roll, gyroXrate, dt); // Calculate the angle using a Kalman filter
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;
}
Serial.println(kalAngleX);
delay(100);
}
Which MPU-6050 library do you use ?
Which Arduino board do you use ?
Which MPU-6050 module do you use, can you give a link to where you bought it ?
Do you know that your MPU6050 might be fake ? It comes in many different flavors of fake. Some sensors do something, some work for a part, some don't work at all. You should do a minimal test with the MPU-6050 to check if it does something.
The issue of the pitch angle being NaN (Not a Number) in your code is likely due to a mathematical error. Specifically, the division by zero in the calculation of the roll angle is causing the problem.
The atan function expects its argument to be a double type, but you are performing integer division, which can result in an incorrect value or zero. When the denominator (sqrt(accX * accX + accZ * accZ)) becomes zero, you are effectively dividing by zero, leading to undefined behavior and producing NaN as the result.
To fix this issue, you need to ensure that the division is performed using double values. You can do this by explicitly casting the variables to double before the division:
By casting accY, accX, and accZ to double, you ensure that the division is performed with floating-point arithmetic, avoiding division by zero.
After making this change, the calculation of the pitch angle should no longer produce NaN, and you should see valid values for the pitch angle in the output.
@fatihbb You are using large libraries which may have issues. The MPU6050 code by ElectronicCats has a copy of the I2Cdev library which as issues.
This is a simpler way (but not the same): https://github.com/jremington/MPU-6050-Fusion
Ive changed the code as you said. I dont know ıf ı made it correct but nan problem is solved. Now I ve another question. Complementary filter's kalman filter's and unfiltered value output is same. İs it possible do you have any idea?
#include <Wire.h>
#include <Kalman.h>
#include <MPU6050.h>
MPU6050 mpu;
Kalman kalmanX; // Create the Kalman instances
Kalman kalmanY;
/* IMU Data */
int16_t _accX, _accY, _accZ;
int16_t _gyroX, _gyroY, _gyroZ;
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;
void setup() {
Serial.begin(115200);
Wire.begin();
mpu.initialize();
delay(100);
}
void loop() {
mpu.getMotion6(&_accX, &_accY, &_accZ, &_gyroX, &_gyroY, &_gyroZ);
double accX = (double) _accX;
double accY = (double) _accY;
double accZ = (double) _accZ;
double gyroX = (double) _gyroX;
double gyroY = (double) _gyroY;
double gyroZ = (double) _gyroZ;
timer = micros();
double dt = (double)(micros() - timer) / 1000000; // Calculate delta time
double roll = atan((double)accY / sqrt((double)(accX * accX) + (double)(accZ * accZ))) * RAD_TO_DEG;
double pitch = atan2(-accX, accZ) * RAD_TO_DEG;
double gyroXrate = gyroX / 131.0; // Convert to deg/s
double gyroYrate = gyroY / 131.0; // Convert to deg/s
kalmanY.setAngle(pitch);
compAngleY = pitch;
kalAngleY = pitch;
gyroYangle = pitch;
kalAngleY = kalmanY.getAngle(pitch, gyroYrate, dt); // Calculate the angle using a Kalman filter
kalmanX.setAngle(roll);
compAngleX = roll;
kalAngleX = roll;
gyroXangle = roll;
kalAngleX = kalmanX.getAngle(roll, gyroXrate, dt); // Calculate the angle using a Kalman filter
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;
}
Serial.print(kalAngleX);
Serial.print(",");
Serial.println(compAngleX);
delay(100);
}
Ive arrenged the code as below complementary filter seems to work good but kalman filter seems to have a huge delay because the output when I roll the mpu6050 ,the kalmans output degreee starts to increase band stops when ı stop rolling but ıt increases very slow like assume mp6050 is rolled to -30 degrees kalman filters output starts from -1.28 to -1.40 in ten seconds.
#include <Wire.h>
#include <Kalman.h>
#include <MPU6050.h>
MPU6050 mpu;
Kalman kalmanX; // Create the Kalman instances
Kalman kalmanY;
/* IMU Data */
int16_t _accX, _accY, _accZ;
int16_t _gyroX, _gyroY, _gyroZ;
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;
void setup() {
Serial.begin(115200);
Wire.begin();
mpu.initialize();
delay(100);
}
void loop() {
mpu.getMotion6(&_accX, &_accY, &_accZ, &_gyroX, &_gyroY, &_gyroZ);
double accX = (double)_accX;
double accY = (double)_accY;
double accZ = (double)_accZ;
double gyroX = (double)_gyroX;
double gyroY = (double)_gyroY;
double gyroZ = (double)_gyroZ;
timer = micros();
double dt = (double)(micros() - timer) / 1000000; // Calculate delta time
double roll = atan((double)accY / sqrt((double)(accX * accX) + (double)(accZ * accZ))) * RAD_TO_DEG;
double pitch = atan2(-accX, accZ) * RAD_TO_DEG;
double gyroXrate = gyroX / 131.0; // Convert to deg/s
double gyroYrate = gyroY / 131.0; // Convert to deg/s
// 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);
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;
}
Serial.print(roll);
Serial.print(",");
Serial.print(kalAngleX);
Serial.print(",");
Serial.println(compAngleX);
delay(100);
}
If you don't understand something, then please ask what I mean.
Did you check the link to the example and compare it with the two lines in your sketch for 'micros()' and 'dt' ?
Spot the difference !
Thanks that solved the problem. Ive seen your warning about micros() order while ım on the phone then on pc ı couldnt see it. So that is why I didnt change it. Thank you again.
It's not a Kalman Filter. The code does something (no one is quite sure what), but not well. The Mahony or Madgwick filters work much better, and the algorithms are well understood.
