Question on Yaw Behavior
Closed this issue · 6 comments
I have been using the Madgwick and Mahoney filters for years and recently have went back to my old stuff to do some more work and found that you updated the filter. So far have done some preliminary testing using the Teensy Propshield from PJRC and a MPU9250 (amazon variety). So far its been working rather well. But with that said I ran into something interesting with the MPU9250 with yaw behavior with the MPU9250: (1) I am seeing yaw going in the opposite direction of movement for a short period of time before coming catching up to the direction of movement and (2) takes a really long time to reach final position after movement.
This is best shown in the following series of charts:
-
Rotation started around 7.5 seconds as can be seen in both the gyro chart and the Euler angles (note the chart in the lower left are a plot of all the flags which remain at 0).
In the figure you can see the yaw movement delayed and moving up and then down. -
Steady state reached in its new position:
Seems to take over 20 seconds to get to where its going.
Not sure what is happening.
I am feeding the algorithm precalibrated values accel and mag. Gyro;s are zeroed out before starting the algorithm so again precalibrated. Calibration takes into account only biases and scale factors. I am running at 100hz.
Also I am using the default settings:
const FusionAhrsSettings settings = {
.convention = FusionConventionEnu,
.gain = 0.5f,
.accelerationRejection = 10.0f,
.magneticRejection = 20.0f,
.rejectionTimeout = 5.0f * SAMPLE_RATE, /* 5 seconds */
};
Any suggestions would be appreciated.
Here is the whole sketch if you want to see whats going on:
#include "MPU9250.h"
#include "Fusion.h"
#include <stdbool.h>
#include <stdio.h>
#include <Wire.h>
#include "EEPROM.h"
#include "Streaming.h"
#include <string>
#define cout Serial
elapsedMillis dump;
float SAMPLE_RATE; // replace this with actual sample RATE in Hz
float g = 9.80665;
float rads2degs = 57.296;
FusionOffset offset;
FusionAhrs ahrs;
FusionQuaternion q;
FusionEuler euler;
FusionAhrsFlags flags;
float val[10];
uint64_t timestamp;
uint8_t raw_values_on = 0;
uint8_t x_values_on = 0;
uint8_t serial_values_on = 0;
uint8_t telem_values_on = 0;
// an MPU9250 object with the MPU-9250 sensor on I2C bus 0 with address 0x68
MPU9250 IMU(Wire,0x68);
int status;
void setup() {
// serial to display data
Serial.begin(57200);
SerialUSB1.begin(57200);
while(!Serial && millis() < 2000) {}
Wire.begin(400000);
// start communication with IMU
status = IMU.begin();
if (status < 0) {
SerialUSB1.println("IMU initialization unsuccessful");
SerialUSB1.println("Check IMU wiring or try cycling power");
SerialUSB1.print("Status: ");
SerialUSB1.println(status);
while(1) {}
}
cout.println("IMU Connected!");
//Load calibration from EEPROM
loadCal();
// setting the accelerometer full scale range to +/-8G
IMU.setAccelRange(MPU9250::ACCEL_RANGE_2G);
// setting the gyroscope full scale range to +/-500 deg/s
IMU.setGyroRange(MPU9250::GYRO_RANGE_2000DPS);
// setting DLPF bandwidth to 20 Hz
IMU.setDlpfBandwidth(MPU9250::DLPF_BANDWIDTH_20HZ);
// setting SRD to 19 for a 50 Hz update rate
// rate = 1000/(SRD + 1) HZ
//IMU.setSrd(19);
//SAMPLE_RATE = 0.02f;
//SRD for 100 hz = 9
IMU.setSrd(9);
SAMPLE_RATE = 0.01f;
// enabling the data ready interrupt
IMU.enableDataReadyInterrupt();
// attaching the interrupt to microcontroller pin 1
pinMode(1,INPUT);
attachInterrupt(1,getIMU,RISING);
//set intial Gyro Calibration
IMU.calibrateGyro();
SerialUSB1.println("Gyro Cal completed");
FusionOffsetInitialise(&offset, SAMPLE_RATE * 100);
FusionAhrsInitialise(&ahrs);
// Set AHRS algorithm settings
const FusionAhrsSettings settings = {
.convention = FusionConventionEnu,
.gain = 0.5f,
.accelerationRejection = 10.0f,
.magneticRejection = 20.0f,
.rejectionTimeout = 5.0f * SAMPLE_RATE, /* 5 seconds */
};
FusionAhrsSetSettings(&ahrs, &settings);
menu();
IMU.calibrateGyro();
SerialUSB1.println("Gyro Cal completed");
}
void loop() {
if ( SerialUSB1.available() ) {
char rr;
rr = SerialUSB1.read();
switch (rr) {
case 'c':
rr ='0';
print_CAL();
break;
case 'r':
{
rr ='0';
if(raw_values_on == 1) {
raw_values_on = 0;
} else if(raw_values_on == 0) {
raw_values_on = 1;
}
}
break;
case 's':
{
rr ='0';
if(serial_values_on == 1) {
serial_values_on = 0;
} else if(serial_values_on == 0) {
serial_values_on = 1;
}
}
break;
case 'x':
{
rr ='0';
if(x_values_on == 1) {
x_values_on = 0;
} else if(serial_values_on == 0) {
x_values_on = 1;
}
}
break;
case 't':
{
rr ='0';
if(telem_values_on == 1) {
telem_values_on = 0;
} else if(telem_values_on == 0) {
telem_values_on = 1;
}
}
break;
case 'g': IMU.calibrateGyro();Serial.println("Gyro Cal completed");break;
case '\r':
case '\n':
case 'h': menu(); break;
}
while (Serial.read() != -1) ; // remove rest of characters.
}
}
void getIMU() {
// read the sensor
IMU.readSensor(val);
// Acquire latest sensor data
FusionVector gyroscope = {val[3]*rads2degs, val[4]*rads2degs, val[5]*rads2degs}; // replace this with actual gyroscope data in degrees/s
FusionVector accelerometer = {val[0]/g, val[1]/g, val[2]/g}; // replace this with actual accelerometer data in g
FusionVector magnetometer = {val[6], val[7], -val[8]}; // replace this with actual magnetometer data in arbitrary units
flags = FusionAhrsGetFlags(&ahrs);
gyroscope = FusionOffsetUpdate(&offset, gyroscope);
// Calculate delta time (in seconds) to account for gyroscope sample clock error
FusionAhrsUpdate(&ahrs, gyroscope, accelerometer, magnetometer, SAMPLE_RATE);
euler = FusionQuaternionToEuler(FusionAhrsGetQuaternion(&ahrs));
q = FusionAhrsGetQuaternion(&ahrs);
//if(dump > 100) {
if(raw_values_on == 1) {
print_float_array(val, 10);
Serial.println();
}
if(serial_values_on == 1) {
Serial.printf("Fusion (RPY): %0.1f, %0.1f, %0.1f\n", euler.angle.roll, euler.angle.pitch, euler.angle.yaw);
}
if(x_values_on == 1) {
timestamp = micros();
char accelgyroBuffer[100];
sprintf(accelgyroBuffer, "%c,%llu,%0.6f,%0.6f,%0.6f,%0.6f,%0.6f,%0.6f", 'I', timestamp, val[3]*rads2degs, val[4]*rads2degs, val[5]*rads2degs, val[0]/g, val[1]/g, val[2]/g);
Serial.printf ("%s\n",accelgyroBuffer);
sprintf(accelgyroBuffer, "%c,%llu,%0.6f,%0.6f,%0.6f", 'M', timestamp, val[6], val[7], -val[8]);
Serial.printf ("%s\n",accelgyroBuffer);
sprintf(accelgyroBuffer, "%c,%llu,%0.6f,%0.6f,%0.6f,%0.6f", 'Q', timestamp, q.element.w, q.element.x, q.element.y, q.element.z);
Serial.printf ("%s\n",accelgyroBuffer);
}
if(telem_values_on == 1) telemetryPortOut();
dump = 0;
//}
}
void loadCal() {
// EEPROM buffer and variables to load accel and mag bias
// and scale factors from CalibrateMPU9250.ino
uint8_t EepromBuffer[48];
float a_xb, a_xs, a_yb, a_ys, a_zb, a_zs;
float hxb, hxs, hyb, hys, hzb, hzs;
// load and set accel and mag bias and scale
// factors from CalibrateMPU9250.ino
for (size_t i = 0; i < sizeof(EepromBuffer); i++) {
EepromBuffer[i] = EEPROM.read(i);
}
memcpy(&a_xb, EepromBuffer + 0, 4);
memcpy(&a_xs, EepromBuffer + 4, 4);
memcpy(&a_yb, EepromBuffer + 8, 4);
memcpy(&a_ys, EepromBuffer + 12, 4);
memcpy(&a_zb, EepromBuffer + 16, 4);
memcpy(&a_zs, EepromBuffer + 20, 4);
memcpy(&hxb, EepromBuffer + 24, 4);
memcpy(&hxs, EepromBuffer + 28, 4);
memcpy(&hyb, EepromBuffer + 32, 4);
memcpy(&hys, EepromBuffer + 36, 4);
memcpy(&hzb, EepromBuffer + 40, 4);
memcpy(&hzs, EepromBuffer + 44, 4);
IMU.setAccelCalX(a_xb, a_xs);
IMU.setAccelCalY(a_yb, a_ys);
IMU.setAccelCalZ(a_zb, a_zs);
IMU.setMagCalX(hxb, hxs);
IMU.setMagCalY(hyb, hys);
IMU.setMagCalZ(hzb, hzs);
}
void print_float_array(float *arr, int size) {
if (size == 0) {
Serial.printf("[]");
} else {
Serial.print('[');
for (int i = 0; i < size - 1; i++)
Serial.printf("%f, ", arr[i]);
Serial.printf("%f]", arr[size - 1]);
}
}
void menu()
{
SerialUSB1.println();
SerialUSB1.println("Menu Options:");
SerialUSB1.println("\tx - x-IMU3 GUI Output");
SerialUSB1.println("\tt - Telemetry Viewer Output");
SerialUSB1.println("\ts - Serial Print Output (Euler Angles)");
SerialUSB1.println("\tc - Print Calibration");
SerialUSB1.println("\tr - Print Raw Values");
SerialUSB1.println("\tg - Zero the Gyros");
SerialUSB1.println("\th - Menu");
SerialUSB1.println();
}
void print_CAL() {
// EEPROM buffer and variables to load accel and mag bias
// and scale factors from CalibrateMPU9250.ino
uint8_t EepromBuffer[48];
float a_xb, a_xs, a_yb, a_ys, a_zb, a_zs;
float hxb, hxs, hyb, hys, hzb, hzs;
// load and set accel and mag bias and scale
// factors from CalibrateMPU9250.ino
for (size_t i = 0; i < sizeof(EepromBuffer); i++) {
EepromBuffer[i] = EEPROM.read(i);
}
memcpy(&a_xb, EepromBuffer + 0, 4);
memcpy(&a_xs, EepromBuffer + 4, 4);
memcpy(&a_yb, EepromBuffer + 8, 4);
memcpy(&a_ys, EepromBuffer + 12, 4);
memcpy(&a_zb, EepromBuffer + 16, 4);
memcpy(&a_zs, EepromBuffer + 20, 4);
memcpy(&hxb, EepromBuffer + 24, 4);
memcpy(&hxs, EepromBuffer + 28, 4);
memcpy(&hyb, EepromBuffer + 32, 4);
memcpy(&hys, EepromBuffer + 36, 4);
memcpy(&hzb, EepromBuffer + 40, 4);
memcpy(&hzs, EepromBuffer + 44, 4);
Serial.println("Accelerometer bias and scale factors:");
cout.print( a_xb, 6); cout.print(", "); Serial.println(a_xs, 6);
cout.print( a_yb, 6); cout.print(", "); Serial.println(a_ys, 6);
cout.print( a_zb, 6); cout.print(", "); Serial.println(a_zs, 6);
Serial.println(" ");
Serial.println("Magnetometer bias and scale factors:");
cout.print( hxb, 6); cout.print(", "); Serial.println(hxs, 6);
cout.print( hyb, 6); cout.print(", "); Serial.println(hys, 6);
cout.print( hzb, 6); cout.print(", "); Serial.println(hzs, 6);
Serial.println(" ");
}
void telemetryPortOut(){
// Set textLength to the number of parameters to print * 31
int textLength = 21 * 31;
char text[textLength];
//Temporary text parameters
char gyroxText[30];
char gyroyText[30];
char gyrozText[30];
char accxText[30];
char accyText[30];
char acczText[30];
char magxText[30];
char magyText[30];
char magzText[30];
char pitchText[30];
char rollText[30];
char yawText[30];
char qwText[30];
char qxText[30];
char qyText[30];
char qzText[30];
char f01[30]; char f02[30]; char f03[30]; char f04[30]; char f05[30];
dtostrf(val[3]*rads2degs, 10, 10, gyroxText);
dtostrf(val[4]*rads2degs, 10, 10, gyroyText);
dtostrf(val[5]*rads2degs, 10, 10, gyrozText);
dtostrf(val[0]/g, 10, 10, accxText);
dtostrf(val[1]/g, 10, 10, accyText);
dtostrf(val[2]/g, 10, 10, acczText);
dtostrf(val[6], 10, 10, magxText);
dtostrf(val[7], 10, 10, magyText);
dtostrf(-val[8], 10, 10, magzText);
dtostrf(euler.angle.pitch, 10, 10, pitchText);
dtostrf(euler.angle.roll, 10, 10, rollText);
dtostrf(euler.angle.yaw, 10, 10, yawText);
dtostrf( q.element.w, 10, 6, qwText);
dtostrf(q.element.x, 10, 6, qxText);
dtostrf(q.element.y, 10, 6, qyText);
dtostrf(q.element.z, 10, 6, qzText);
dtostrf((float)flags.initialising, 10, 6, f01);
dtostrf((float)flags.accelerationRejectionWarning, 10, 6, f02);
dtostrf((float)flags.accelerationRejectionTimeout, 10, 6, f03);
dtostrf((float)flags.magneticRejectionWarning, 10, 6, f04);
dtostrf((float)flags.magneticRejectionTimeout, 10, 6, f05);
// Create single text parameter and print it
snprintf(text, textLength, "%s, %s, %s,%s,%s,%s,%s,%s,%s,%s,%s,%s,%s,%s,%s,%s,%s,%s,%s,%s,%s",
gyroxText, gyroyText, gyrozText,
accxText, accyText, acczText,
magxText, magyText, magzText,
pitchText, rollText, yawText,
qwText, qxText, qyText, qzText,
f01, f02, f03, f04, f05);
Serial.println(text);
//Serial.send_now();
}
I expect your magnetometer needs calibrating. I suggest you use FusionAhrsUpdateNoMagnetometer
instead of FusionAhrsUpdate
to see the behaviour without the magnetometer. You may also wish to plot the output of FusionCompassCalculateHeading
for known rotations to see exactly what heading (yaw) measurement your magnetometer is providing.
Thanks for the fast response and will give those suggestions a try and see how it goes. Have to set up a rotation rose on the desk :) Project for tomorrow. Getting late here now. Will let you know how it goes
I expect your magnetometer needs calibrating. I suggest you use
FusionAhrsUpdateNoMagnetometer
instead ofFusionAhrsUpdate
to see the behaviour without the magnetometer. You may also wish to plot the output ofFusionCompassCalculateHeading
for known rotations to see exactly what heading (yaw) measurement your magnetometer is providing.
Took your advice and went back to looking at calibration methods. Found I had one issue with the way I was doing the calibration which I fixed and also switched to using NED coordinates to match what the how the library I am using has the axes aligned.
That seemed to fix a couple of issues I was having (yes I am using a compass rose as well to check yaw and heading). However with that said I am still seing a lag in yaw as shown in the next too figures using the default settings:
const FusionAhrsSettings settings = {
.convention = FusionConventionEnu,
.gain = 0.5f,
.accelerationRejection = 10.0f,
.magneticRejection = 20.0f,
.rejectionTimeout = 5.0f * SAMPLE_RATE, /* 5 seconds */
};
from the figures you can see it took about 12 seconds for yaw to converge.
Now if I change the settings to:
const FusionAhrsSettings settings = {
.convention = FusionConventionNed,
.gain = 1.5f,
.accelerationRejection = 10.0f,
.magneticRejection = 5.0f,
.rejectionTimeout = 5.0f * SAMPLE_RATE, /* 5 seconds */
};
Am I on the right track here. And is there any other guidance on adjusting the settings that you can provide other than what is in the READ.ME?
Your plots are of little use because you have not provided any indication what the actual rotation is. You need to compare your measured rotation with the actual rotation. For example, rotate the device 90° and compare this with the measured rotation.
Your plots are of little use because you have not provided any indication what the actual rotation is. You need to compare your measured rotation with the actual rotation. For example, rotate the device 90° and compare this with the measured rotation.
Note: working on my desk and now looking at it like this still looks off a bit I think. Well back to calibration.
Sorry thought I mentioned that, hope this is what you are talking about
Good luck with calibration. I will now close this issue.