Hi all,
I am running code that reads data from a BNO055 9-DOF sensor. The sensor supplies a.o. acceleration-data, gyro-data and magnetometer-data. The acceleration data can be used to calculate tilt-angles of the X-axis and Y-axis of the sensor. This works fine.
Unfortunately, movement in X-, Y-, and Z- direction cause errors in calculated tilt-angles and to minimize this effect a low-pass filter can be applied.
The low-pass filter calculates a (current) filtered value based on a currently measured value and previously filtered value. At the next measurement, the (current) filtered value serves as the previously filtered value.
In code this means that the filtered value is calculated in the current run of the loop and after the calculation it has to be stored as “previously filtered value”. In the next run of the loop serves that as the previously filtered value.
THE PROBLEM
The problem I run into now is that when I assign the filtered values (theta_filtered_new and phi_filtered_new) to the “previously filtered values”(theta_filtered_old and phi_filtered_old), the filtered values return nan-values.
I have two questions:
- What can be the cause of this problem? As far as I can see there is nothing wrong with the code, but maybe I have overlooked something.
- How can this problem be solved?
Any help would greatly be appreciated.
Please find the working code, the erraneous code and the outputs both code sections produce. The code is directly uploadable into an Arduino Uno.
The code that causes the nan-values is:
// -----------------------------------------
// nan-ERROR
// - When uncommenting the lines below,
// the calculated values become nan-values
// in the next run of the loop
// -----------------------------------------
//theta_filtered_old = theta_filtered_new;
//phi_filtered_old = phi_filtered_new;
I am running the following arduino programming IDE (obtained from "Help") on an Arduino Uno.
Version: 2.0.3
Date: 2022-12-05T09:30:25.331Z
CLI Version: 0.29.0 [76251df9]
Copyright © 2023 Arduino SA
Unfortunately I cannot upload the attachments yet because I am a new user.
Working Code
Note that the updates of theta_filtered_old and phi_filtered_old are flagged off.
// File : LoopProblem_Working_Code.ino
// Problem : Updating a loop variable results in nan-values
#include <Wire.h>
#include <Adafruit_Sensor.h>
#include <Adafruit_BNO055.h>
#include <utility/imumaths.h>
#define BNO055_SAMPLERATE_DELAY_MS (100)
Adafruit_BNO055 myIMU = Adafruit_BNO055();
int loopctr = 0;
float gX;
float gY;
float gZ;
float gLen;
float gLen_n;
float theta_M; // Tilt angle of the X-axis (calculated from measured values)
float phi_M; // Tilt angle of the Y-axis (calculated from measured values)
// Variables for the low-pass filter
float theta_filtered_old = 1.0; // previously calculated filtered value (in previous run of the loop)
float theta_filtered_new = 1.0; // currently calculated filtered value (in current run of the loop)
float phi_filtered_old = 1.0; // previously calculated filtered value (in previous run of the loop)
float phi_filtered_new = 1.0; // currently calculated filtered value (in current run of the loop)
float p1 = 0.9; // Filter factor to apply to the "old values" of theta and phi
float p2 = 0.1; // Filter factor to apply to the "new values" of theta and phi
void setup() {
// put your setup code here, to run once:
Serial.begin(115200);
myIMU.begin();
delay(1000);
int8_t temp = myIMU.getTemp();
Serial.println(temp);
myIMU.setExtCrystalUse(true);
}
void loop() {
//============================================
// --- PROCESS VARIABLES
//============================================
uint8_t system, gro, accel, mg = 0;
//============================================
// --- DO LOOP-ADMINISTRATION
//============================================
loopctr += 1;
//Serial.print(loopctr); Serial.print (",");
//============================================
// --- DO MEASUREMENTS
//============================================
// --- Get calibration values
myIMU.getCalibration(&system, &gro, &accel, &mg);
// --- Read the values from the BNO055
imu::Vector<3> acc = myIMU.getVector(Adafruit_BNO055::VECTOR_ACCELEROMETER);
//imu::Vector<3> gyro = myIMU.getVector(Adafruit_BNO055::VECTOR_GYROSCOPE);
//imu::Vector<3> mag = myIMU.getVector(Adafruit_BNO055::VECTOR_MAGNETOMETER);
//============================================
// DO CALCULATIONS
//============================================
// --- Normalise data usch that everything is referenced "to 1G"
gLen =sqrt(acc.x()*acc.x() + acc.y()*acc.y() + acc.z()*acc.z());
gX = acc.x() / gLen;
gY = acc.y() / gLen;
gZ = acc.z() / gLen;
gLen_n = gLen / 9.8;
// ---convert measurements to angles in degrees
theta_M = atan2(gX,gZ) * 180/3.14159265;
phi_M = atan2(gY,gZ) * 180/3.14159265;
// --- Apply the low-pass filter
//theta_filtered_new = p1*theta_filtered_old + p2*theta_M;
//phi_filtered_new = p1*phi_filtered_old + p2*phi_M;
theta_filtered_new = p1*theta_filtered_old + p2*theta_M;
phi_filtered_new = p1*phi_filtered_old + p2*phi_M;
//============================================
// --- BUILD THE DATA STRING
// --- Current #elements : 14
//============================================
// --- Display calibration data
Serial.print(accel);
Serial.print(",");
Serial.print(gro);
Serial.print(",");
Serial.print(mg);
Serial.print(",");
Serial.print(system);
Serial.print(", ");
// --- Display Measured data
Serial.print(gX); Serial.print(",");
Serial.print(gY); Serial.print(",");
Serial.print(gZ); Serial.print(",");
Serial.print(gLen_n); Serial.print(",");
Serial.print(theta_M); Serial.print(",");
Serial.print(phi_M); Serial.print(",");
Serial.print(theta_filtered_new); Serial.print(",");
Serial.print(phi_filtered_new); Serial.print(",");
// -----------------------------------------
// nan-ERROR
// - When uncommenting the lines below,
// the calculated values become nan-values
// in the next run of the loop
// -----------------------------------------
//theta_filtered_old = theta_filtered_new;
//phi_filtered_old = phi_filtered_new;
Serial.print(theta_filtered_old); Serial.print(",");
Serial.print(phi_filtered_old);
// Close data string and send to COM-port
Serial.println("");
delay(BNO055_SAMPLERATE_DELAY_MS);
}
Output from Working code:
timestamp,calib_acc,calib_gyro,calib_magneto,calib_System,gX (norm),gY (norm),gZ (norm),gLen,Theta_M,Phi_M,Theta_filtered_new,Phi_filtered_new,theta_filtered_old,phi_filtered_old
1676817172833,3.00,3.00,3.00,3.00,0.00,0.01,1.00,0.99,0.06,0.41,0.91,0.94,1.00,1.00
1676817172936,3.00,3.00,3.00,3.00,0.00,0.00,1.00,0.99,0.06,0.24,0.91,0.92,1.00,1.00
1676817173043,3.00,3.00,3.00,3.00,0.00,0.01,1.00,0.99,0.06,0.30,0.91,0.93,1.00,1.00
1676817173148,3.00,3.00,3.00,3.00,0.00,0.01,1.00,0.99,0.06,0.35,0.91,0.94,1.00,1.00
1676817173257,3.00,3.00,3.00,3.00,0.00,0.00,1.00,0.99,0.00,0.18,0.90,0.92,1.00,1.00
1676817173362,3.00,3.00,3.00,3.00,0.00,0.01,1.00,0.99,0.12,0.30,0.91,0.93,1.00,1.00
1676817173468,3.00,3.00,3.00,3.00,0.00,0.00,1.00,0.99,0.18,0.06,0.92,0.91,1.00,1.00
1676817173574,3.00,3.00,3.00,3.00,0.00,0.01,1.00,0.99,0.06,0.30,0.91,0.93,1.00,1.00
1676817173682,3.00,3.00,3.00,3.00,0.00,0.01,1.00,0.99,0.24,0.35,0.92,0.94,1.00,1.00
1676817173788,3.00,3.00,3.00,3.00,0.00,0.00,1.00,0.99,0.06,0.24,0.91,0.92,1.00,1.00
1676817173892,3.00,3.00,3.00,3.00,0.00,0.00,1.00,0.99,0.12,0.12,0.91,0.91,1.00,1.00
1676817173998,3.00,3.00,3.00,3.00,0.00,0.00,1.00,0.99,0.12,0.18,0.91,0.92,1.00,1.00
1676817174107,3.00,3.00,3.00,3.00,0.00,0.00,1.00,0.99,0.00,0.24,0.90,0.92,1.00,1.00
1676817174210,3.00,3.00,3.00,3.00,0.00,0.00,1.00,0.99,0.06,0.18,0.91,0.92,1.00,1.00
1676817174316,3.00,3.00,3.00,3.00,0.00,0.00,1.00,0.99,0.06,0.24,0.91,0.92,1.00,1.00
1676817174419,3.00,3.00,3.00,3.00,0.00,0.00,1.00,0.99,0.18,0.18,0.92,0.92,1.00,1.00
1676817174529,3.00,3.00,3.00,3.00,0.00,0.00,1.00,0.99,0.12,0.18,0.91,0.92,1.00,1.00
1676817174636,3.00,3.00,3.00,3.00,0.00,0.00,1.00,0.99,0.06,0.18,0.91,0.92,1.00,1.00
1676817174740,3.00,3.00,3.00,3.00,0.00,0.00,1.00,0.99,0.12,0.24,0.91,0.92,1.00,1.00
1676817174848,3.00,3.00,3.00,3.00,0.00,0.01,1.00,0.99,0.06,0.35,0.91,0.94,1.00,1.00
1676817174956,3.00,3.00,3.00,3.00,0.00,0.00,1.00,0.99,0.06,0.18,0.91,0.92,1.00,1.00
1676817175062,3.00,3.00,3.00,3.00,0.00,0.00,1.00,0.99,0.12,0.24,0.91,0.92,1.00,1.00
1676817175168,3.00,3.00,3.00,3.00,0.00,0.00,1.00,0.99,0.12,0.24,0.91,0.92,1.00,1.00
...
...
Erraneous Code
Note that the updates of theta_filtered_old and phi_filtered_old are activated.
// File:LoopProblem_Erraneous_Code.ino
// Problem : Updating a loop variable results in nan-values
#include <Wire.h>
#include <Adafruit_Sensor.h>
#include <Adafruit_BNO055.h>
#include <utility/imumaths.h>
#define BNO055_SAMPLERATE_DELAY_MS (100)
Adafruit_BNO055 myIMU = Adafruit_BNO055();
int loopctr = 0;
float gX;
float gY;
float gZ;
float gLen;
float gLen_n;
float theta_M; // Tilt angle of the X-axis (calculated from measured values)
float phi_M; // Tilt angle of the Y-axis (calculated from measured values)
// Variables for the low-pass filter
float theta_filtered_old = 1.0; // previously calculated filtered value (in previous run of the loop)
float theta_filtered_new = 1.0; // currently calculated filtered value (in current run of the loop)
float phi_filtered_old = 1.0; // previously calculated filtered value (in previous run of the loop)
float phi_filtered_new = 1.0; // currently calculated filtered value (in current run of the loop)
float p1 = 0.9; // Filter factor to apply to the "old values" of theta and phi
float p2 = 0.1; // Filter factor to apply to the "new values" of theta and phi
void setup() {
// put your setup code here, to run once:
Serial.begin(115200);
myIMU.begin();
delay(1000);
int8_t temp = myIMU.getTemp();
Serial.println(temp);
myIMU.setExtCrystalUse(true);
}
void loop() {
//============================================
// --- PROCESS VARIABLES
//============================================
uint8_t system, gro, accel, mg = 0;
//============================================
// --- DO LOOP-ADMINISTRATION
//============================================
loopctr += 1;
//Serial.print(loopctr); Serial.print (",");
//============================================
// --- DO MEASUREMENTS
//============================================
// --- Get calibration values
myIMU.getCalibration(&system, &gro, &accel, &mg);
// --- Read the values from the BNO055
imu::Vector<3> acc = myIMU.getVector(Adafruit_BNO055::VECTOR_ACCELEROMETER);
//imu::Vector<3> gyro = myIMU.getVector(Adafruit_BNO055::VECTOR_GYROSCOPE);
//imu::Vector<3> mag = myIMU.getVector(Adafruit_BNO055::VECTOR_MAGNETOMETER);
//============================================
// DO CALCULATIONS
//============================================
// --- Normalise data usch that everything is referenced "to 1G"
gLen =sqrt(acc.x()*acc.x() + acc.y()*acc.y() + acc.z()*acc.z());
gX = acc.x() / gLen;
gY = acc.y() / gLen;
gZ = acc.z() / gLen;
gLen_n = gLen / 9.8;
// ---convert measurements to angles in degrees
theta_M = atan2(gX,gZ) * 180/3.14159265;
phi_M = atan2(gY,gZ) * 180/3.14159265;
// --- Apply the low-pass filter
//theta_filtered_new = p1*theta_filtered_old + p2*theta_M;
//phi_filtered_new = p1*phi_filtered_old + p2*phi_M;
theta_filtered_new = p1*theta_filtered_old + p2*theta_M;
phi_filtered_new = p1*phi_filtered_old + p2*phi_M;
//============================================
// --- BUILD THE DATA STRING
// --- Current #elements : 14
//============================================
// --- Display calibration data
Serial.print(accel);
Serial.print(",");
Serial.print(gro);
Serial.print(",");
Serial.print(mg);
Serial.print(",");
Serial.print(system);
Serial.print(", ");
// --- Display Measured data
Serial.print(gX); Serial.print(",");
Serial.print(gY); Serial.print(",");
Serial.print(gZ); Serial.print(",");
Serial.print(gLen_n); Serial.print(",");
Serial.print(theta_M); Serial.print(",");
Serial.print(phi_M); Serial.print(",");
Serial.print(theta_filtered_new); Serial.print(",");
Serial.print(phi_filtered_new); Serial.print(",");
// -----------------------------------------
// nan-ERROR
// - The lines below are "active"
// the calculated values become nan-values
// in the next run of the loop
// -----------------------------------------
theta_filtered_old = theta_filtered_new;
phi_filtered_old = phi_filtered_new;
Serial.print(theta_filtered_old); Serial.print(",");
Serial.print(phi_filtered_old);
// Close data string and send to COM-port
Serial.println("");
delay(BNO055_SAMPLERATE_DELAY_MS);
}
Output from Erraneous code:
timestamp,calib_acc,calib_gyro,calib_magneto,calib_System,gX (norm),gY (norm),gZ (norm),gLen,Theta_M,Phi_M,Theta_filtered_new,Phi_filtered_new,theta_filtered_old,phi_filtered_old
1676817465388,3.00,3.00,3.00,3.00,0.01,-0.01,1.00,1.00,0.35,-0.76,nan,nan,nan,nan
1676817465480,3.00,3.00,3.00,3.00,0.01,-0.01,1.00,1.00,0.41,-0.82,nan,nan,nan,nan
1676817465593,3.00,3.00,3.00,3.00,0.01,-0.01,1.00,1.00,0.41,-0.70,nan,nan,nan,nan
1676817465704,3.00,3.00,3.00,3.00,0.01,-0.01,1.00,1.00,0.35,-0.76,nan,nan,nan,nan
1676817465801,3.00,3.00,3.00,3.00,0.01,-0.01,1.00,1.00,0.29,-0.82,nan,nan,nan,nan
1676817465913,3.00,3.00,3.00,3.00,0.01,-0.01,1.00,1.00,0.35,-0.76,nan,nan,nan,nan
1676817466014,3.00,3.00,3.00,3.00,0.01,-0.01,1.00,1.00,0.41,-0.64,nan,nan,nan,nan
1676817466126,3.00,3.00,3.00,3.00,0.01,-0.01,1.00,1.00,0.29,-0.82,nan,nan,nan,nan
1676817466232,3.00,3.00,3.00,3.00,0.01,-0.01,1.00,1.00,0.35,-0.76,nan,nan,nan,nan
1676817466333,3.00,3.00,3.00,3.00,0.01,-0.01,1.00,1.00,0.35,-0.82,nan,nan,nan,nan
1676817466440,3.00,3.00,3.00,3.00,0.00,-0.01,1.00,1.00,0.12,-0.82,nan,nan,nan,nan
1676817466541,3.00,3.00,3.00,3.00,0.01,-0.01,1.00,1.00,0.35,-0.76,nan,nan,nan,nan
1676817466642,3.00,3.00,3.00,3.00,0.01,-0.01,1.00,1.00,0.47,-0.82,nan,nan,nan,nan
1676817466754,3.00,3.00,3.00,3.00,0.01,-0.01,1.00,1.00,0.41,-0.76,nan,nan,nan,nan
1676817466855,3.00,3.00,3.00,3.00,0.01,-0.01,1.00,1.00,0.35,-0.76,nan,nan,nan,nan
1676817466967,3.00,3.00,3.00,3.00,0.01,-0.01,1.00,1.00,0.29,-0.70,nan,nan,nan,nan
1676817467068,3.00,3.00,3.00,3.00,0.01,-0.01,1.00,1.00,0.41,-0.76,nan,nan,nan,nan
1676817467170,3.00,3.00,3.00,3.00,0.00,-0.01,1.00,1.00,0.23,-0.70,nan,nan,nan,nan
1676817467281,3.00,3.00,3.00,3.00,0.01,-0.01,1.00,1.00,0.47,-.76,nan,nan,nan,nan
...
...