Hello there,
I started using Adafruit 10 DOF IMU breakout for a while and is stuck with a problem that needs urgent solution. The problem I am facing is while using the AHRS code for 10DOF provided by Adafruit.
During the reading if I rotate the sensor along z-axis, I get close to zero values in pitch and roll and an angle count in yaw.
Also while tilting the sensor along the y-axis, I get close to zero values in pitch and yaw and an angle count in roll.
However, while taking the reading along the x-axis, I get a lot of drifting values in yaw and roll which does not make sense to me any more. I have attached pictures. Should the result not be close to zero values for yaw and roll and an angle count for pitch?
I will really appreciate help in this matter.
Problem in reading for AHRS of Adafruit 10 DOF IMU
Moderators: adafruit_support_bill, adafruit
Please be positive and constructive with your questions and comments.
- Hambalala
- Posts: 9
- Joined: Sun Feb 08, 2015 2:56 pm
Problem in reading for AHRS of Adafruit 10 DOF IMU
- Attachments
-
- This reading is taken by putting the sensor tilting along -(ve) x axis
- Tilt data 2.PNG (43.97 KiB) Viewed 2429 times
-
- This reading is taken by putting the sensor tilting along +(ve) x axis
- Tilt data.PNG (46.35 KiB) Viewed 2429 times
- ktownsend
- Posts: 1447
- Joined: Thu Nov 05, 2009 2:18 am
Re: Problem in reading for AHRS of Adafruit 10 DOF IMU
The drift you see is most likely caused by the fact that the magnetometer used in the calculations is not calibrated for hard iron or soft iron errors. We have plans to introduce a calibration utility as well as integrate the gyroscope data (using a common algorithm like Magdwick, etc.), but at present we don't have an example offering full integration with this chipset.
I'll try to move this up the ToDo list since it's been sitting there too long.
I'll try to move this up the ToDo list since it's been sitting there too long.
- Hambalala
- Posts: 9
- Joined: Sun Feb 08, 2015 2:56 pm
Re: Problem in reading for AHRS of Adafruit 10 DOF IMU
I appreciate your reply. Can you please suggest me a step-wise procedure of approaching this problem with other kind of algorithm? and what kind of calibration utility I could use in this case?
- ktownsend
- Posts: 1447
- Joined: Thu Nov 05, 2009 2:18 am
Re: Problem in reading for AHRS of Adafruit 10 DOF IMU
There are a lot of DOF solutions out there. I'd suggest doing a quick search for "Madgwick" plus Arduino since that's probably the most appropriate algorithm for 9DOF sensor fusion. It's light weight (compared to other solutions) and several code examples are floating around out there.
Here is an explanation of how calibration works for magnetometers, and this blog is also an excellent place to start looking at some of the implementation details around AHRS: http://www.camelsoftware.com/firetail/b ... ft-errors/
K.
Here is an explanation of how calibration works for magnetometers, and this blog is also an excellent place to start looking at some of the implementation details around AHRS: http://www.camelsoftware.com/firetail/b ... ft-errors/
K.
- Hambalala
- Posts: 9
- Joined: Sun Feb 08, 2015 2:56 pm
Re: Problem in reading for AHRS of Adafruit 10 DOF IMU
Thanks for your support. Just a question: Do you have idea on when you guys are going to implement it?
- waltmck
- Posts: 25
- Joined: Sat Jul 21, 2012 11:54 am
Re: Problem in reading for AHRS of Adafruit 10 DOF IMU
ktownsend said
Doing the calibrations and using the Mahoney algorithm I'm getting accurate and steady readings for roll and pitch from 0 to +/- 60 degrees. Less luck so far with the Madgwick.
I'll second getting this is place. Mainly the ability to feed calibration values back into the libraries. Without the ability to do this I've found the pitch/roll/orientation functions to be not very useful. My sensor gives a steady roll of 6 degrees when sitting on my fairly level window sill.We have plans to introduce a calibration utility as well as integrate the gyroscope data (using a common algorithm like Magdwick, etc.), but at present we don't have an example offering full integration with this chipset.
I'll try to move this up the ToDo list since it's been sitting there too long.
Doing the calibrations and using the Mahoney algorithm I'm getting accurate and steady readings for roll and pitch from 0 to +/- 60 degrees. Less luck so far with the Madgwick.
- Hambalala
- Posts: 9
- Joined: Sun Feb 08, 2015 2:56 pm
Re: Problem in reading for AHRS of Adafruit 10 DOF IMU
Hey waltmck, how are you using the Mahony algorithm?
- waltmck
- Posts: 25
- Joined: Sat Jul 21, 2012 11:54 am
Re: Problem in reading for AHRS of Adafruit 10 DOF IMU
Hambalala
By how, I'm not sure if you mean for what purpose am I using it, or what is the specific coding I'm using.
For the purpose, the Mahoney and Madgwick filters are used to smooth out the sensor data in a dynamic environment. After calibrating the sensors, I was able to copy the roll and pitch algorithms out of the Adafruit libraries and I got quite accurate roll and pitch (down to less than 0.5 degree) in a static environment. But if I then put some lateral acceleration, the values got real noisy.
The Mahoney filter has smoothed things out quite a bit, I can put lateral acceleration and still get a fairly smooth value. How smooth depends on how I set the Kp gain factor for the algorithm. If it set it low, I only get a small amount of noise but it takes lower for the filter to converge on the correct angle. If I set it higher, it converges on the angle quicker but has more noise.
For the how, I copied the C code for both the Madgwick and Mahoney filters from the web site about the Madgwick algorithm (http://www.x-io.co.uk/open-source-imu-a ... lgorithms/ . I only copied the .c files and not the .h files. I put the .c file in a new tab in the sketch where I'm reading the sensors. I commented out the header file in his .c code and moved the quaternion and sample frequency variable to my sensor sketch. Then I just call his routine using my calibrated sensor values.
It is important that your sensor values are calibrated. I did a pretty good calibration of mine, but I still needed to adjust them slightly after I started calling the algorithm. Things I've seen say that the gyro readings need to be in rad/sec, but that acceleration and mag values are unit neutral. I haven't found that to be true for acceleration. I've needed to convert the acceleration from the m/s^2 that the library gives back to milliGs to get it to work right. This may be due to my not quite understanding things and how to use the algorithm. I've tried a few sampling rates and settled on 25 Hz. I've heard that it works well down to 10 Hz.
The Madgwick filter seems to work better in that I get less noise than I get from the Mahoney filter under dynamic load. But so far, I haven't got it to give me the correct angle value. I'm getting about two-thirds what the angle should be. For instance, if I incline it to 45 degrees, I get about 30 degrees as a result. This is consistent enough that it feels like a calibration error or error in my calculations that I haven't found yet.
I'm also running my gyro readings through a running-median filter before sending them to the Mahoney algorithm. This is because I'm getting an intermittent spike on my yAxis reading from the sensor. It happens for just one reading and then goes away and it happens at random intervals. The running-median filter removes these.
The filters give you a quaternion as a result, so I then use the equations that convert this into Euler angles for roll, pitch and yaw.
I haven't been paying much attention to yaw as it's not important to me. The last time I looked, I was getting drift in it, but I haven't looked since adjusting my calibration values.
By how, I'm not sure if you mean for what purpose am I using it, or what is the specific coding I'm using.
For the purpose, the Mahoney and Madgwick filters are used to smooth out the sensor data in a dynamic environment. After calibrating the sensors, I was able to copy the roll and pitch algorithms out of the Adafruit libraries and I got quite accurate roll and pitch (down to less than 0.5 degree) in a static environment. But if I then put some lateral acceleration, the values got real noisy.
The Mahoney filter has smoothed things out quite a bit, I can put lateral acceleration and still get a fairly smooth value. How smooth depends on how I set the Kp gain factor for the algorithm. If it set it low, I only get a small amount of noise but it takes lower for the filter to converge on the correct angle. If I set it higher, it converges on the angle quicker but has more noise.
For the how, I copied the C code for both the Madgwick and Mahoney filters from the web site about the Madgwick algorithm (http://www.x-io.co.uk/open-source-imu-a ... lgorithms/ . I only copied the .c files and not the .h files. I put the .c file in a new tab in the sketch where I'm reading the sensors. I commented out the header file in his .c code and moved the quaternion and sample frequency variable to my sensor sketch. Then I just call his routine using my calibrated sensor values.
It is important that your sensor values are calibrated. I did a pretty good calibration of mine, but I still needed to adjust them slightly after I started calling the algorithm. Things I've seen say that the gyro readings need to be in rad/sec, but that acceleration and mag values are unit neutral. I haven't found that to be true for acceleration. I've needed to convert the acceleration from the m/s^2 that the library gives back to milliGs to get it to work right. This may be due to my not quite understanding things and how to use the algorithm. I've tried a few sampling rates and settled on 25 Hz. I've heard that it works well down to 10 Hz.
The Madgwick filter seems to work better in that I get less noise than I get from the Mahoney filter under dynamic load. But so far, I haven't got it to give me the correct angle value. I'm getting about two-thirds what the angle should be. For instance, if I incline it to 45 degrees, I get about 30 degrees as a result. This is consistent enough that it feels like a calibration error or error in my calculations that I haven't found yet.
I'm also running my gyro readings through a running-median filter before sending them to the Mahoney algorithm. This is because I'm getting an intermittent spike on my yAxis reading from the sensor. It happens for just one reading and then goes away and it happens at random intervals. The running-median filter removes these.
The filters give you a quaternion as a result, so I then use the equations that convert this into Euler angles for roll, pitch and yaw.
I haven't been paying much attention to yaw as it's not important to me. The last time I looked, I was getting drift in it, but I haven't looked since adjusting my calibration values.
- Hambalala
- Posts: 9
- Joined: Sun Feb 08, 2015 2:56 pm
Re: Problem in reading for AHRS of Adafruit 10 DOF IMU
Thanks for your suggestions! Is it possible for you to share your code so that it can help me make the process easier?
- waltmck
- Posts: 25
- Joined: Sat Jul 21, 2012 11:54 am
Re: Problem in reading for AHRS of Adafruit 10 DOF IMU
This isn't my main program. I'm combining the sensors with a GPS logger for that. This is a program that I used to test the calculations. I got one of Adafruit's smallpan/tilt servo setups and mounted the sensors on it. The picture doesn't show it mounted. I've taken it off to do my final packaging. I had to nip the wings on the pan/tilt so the sensor board would snap in.
What I've done, separately, is run the pan/tilt without any calculations and just logged the raw values and the servo readings for twenty minutes. At some point I'll probably come back and process that data off line.
The sketch for the pan/tilt has three tabs. The main code tab, a tab for the Mahoney code and a tab for the Madgwick code.
The main code is:
The Madgwick code is:
The Mahoney code is:
I used the pan/tilt to move the sensor through +/- 20 degrees in roll and pitch then printed out the results. It does show the calculations. I'm getting fair results but haven't played with the parameters. I've got to the point where "good enough and the enemy of better" on this project. it will do what I really need for this summer, but it's not as good as I would like. What I've done, separately, is run the pan/tilt without any calculations and just logged the raw values and the servo readings for twenty minutes. At some point I'll probably come back and process that data off line.
The sketch for the pan/tilt has three tabs. The main code tab, a tab for the Mahoney code and a tab for the Madgwick code.
The main code is:
Code: Select all
//This sketch is setup to run on an Arduino MEGA
//
//Will move the sensor through about + and - 20 degree pitch and roll
//and report out the angles
//
#include <SPI.h>
#include <Wire.h>
#include <Servo.h>
Servo servoX;
Servo servoY;
#include <Adafruit_10DOF.h> // Adafruit wrapper for the 10 DOF board
#include <Adafruit_Sensor.h> // Adafruit unifed sensor wrapper
#include <Adafruit_LSM303_U.h> // Adafruit LSM303 accelerometer and magnometer
#include <Adafruit_BMP085_U.h> // Adafruit pressure and tempeture sensor
#include <Adafruit_L3GD20_U.h> // Adafruit L3GD20 gyro
Adafruit_10DOF dof = Adafruit_10DOF();
Adafruit_LSM303_Accel_Unified accel = Adafruit_LSM303_Accel_Unified(30301);
Adafruit_LSM303_Mag_Unified mag = Adafruit_LSM303_Mag_Unified(30302);
Adafruit_BMP085_Unified bmp = Adafruit_BMP085_Unified(18001);
Adafruit_L3GD20_Unified gyro = Adafruit_L3GD20_Unified(20);
#include <avr/sleep.h> // Needed for timer interrupt
#include <RunningMedian.h>
RunningMedian xGyroMedianArray = RunningMedian(5);
RunningMedian yGyroMedianArray = RunningMedian(5);
RunningMedian zGyroMedianArray = RunningMedian(5);
RunningMedian xAccelMedianArray = RunningMedian(10);
RunningMedian yAccelMedianArray = RunningMedian(10);
RunningMedian zAccelMedianArray = RunningMedian(10);
// Set the pins used
const int slaveSelect = 53;
const int chipSelect = 10;
volatile unsigned int millisCnt = 0;
int servoPinX = 44;
int servoPinY = 45;
int servoAngleY = 0; // servo position in degrees
int servo0Y = 85; // assumed level value
int servoMinY = 65; // lowest angle to go to
int servoMaxY = 105; // highest angle to go to
int servoTimerTestY = 5; // numbers of interrupts to count before change
int servoTimerCntY = 0; // number of triggers
int servoAngleX = 0; // servo position in degrees
int servo0X = 100; // assumed level value
int servoMinX = 80; // lowest angle to go to
int servoMaxX = 120; // highest angle to go to
int servoTimerTestX = 2; // number of triggers to count before change
int servoTimerCntX = 0; // number of triggers
boolean servoXDirectionPositive = true;
boolean servoYDirectionPositive = true;
// Calibration values for accel
float accelFactor = 100.937; // Convert gravity in m/sec^2 to milliGs
float xAccelScale = 1.003; // My calculated xAxis scale
float xAccelOffset = 0.095; // My calculated xAxis offset
float yAccelScale = 0.979; // My calculated yAxis scale
float yAccelOffset = 0.47; // My calculated yAxis offset
float zAccelScale = 0.982; // My calculated zAxis scale
float zAccelOffset = -0.615; // My calculated zAxis offset
// Calibration values for gyro
float xGyroOffset = -0.0102;
float yGyroOffset = -0.0403;
float zGyroOffset = -0.0748;
float xGyroScale = 1.0069;
float yGyroScale = 1.0241;
float zGyroScale = 0.9959;
// Calibration values for magnetometer
float xMagOffset = -2.59;
float yMagOffset = 3.135;
float zMagOffset = 0.765;
float xMagScale = 1;
float yMagScale = 0.9806;
float zMagScale = 0.9474;
// Sensor raw readings
float xAccelRaw = 0.0; // Reported xAxis value
float yAccelRaw = 0.0; // Reported yAxis value
float zAccelRaw = 0.0; // Reported zAxis value
float xGyroRaw = 0.0;
float yGyroRaw = 0.0;
float zGyroRaw = 0.0;
float xMagRaw = 0.0;
float yMagRaw = 0.0;
float zMagRaw = 0.0;
// Accel calibrated values
float xAccelCal = 0.0; // My calibated xAxix value
float yAccelCal = 0.0; // My calibrated yAxis value
float zAccelCal = 0.0; // My calibrated zAxis value
float xAccelMedian = 0.0;
float yAccelMedian = 0.0;
float zAccelMedian = 0.0;
float xGyroCal = 0.0;
float yGyroCal = 0.0;
float zGyroCal = 0.0;
float xGyroMedian = 0.0;
float yGyroMedian = 0.0;
float zGyroMedian = 0.0;
float xMagCal = 0.0;
float yMagCal = 0.0;
float zMagCal = 0.0;
float unfilteredRoll = 0.0;
float unfilteredPitch = 0.0;
float intermediateValue = 0;
float unfilteredYaw = 0.0;
float madgwickRoll = 0.0;
float madgwickPitch = 0.0;
float madgwickYaw = 0.0;
float mahoneyRoll = 0.0;
float mahoneyPitch = 0.0;
float mahoneyYaw = 0.0;
float radToDegree = 57.2957795; // Factor to convet radians to degrees
float knotsToMPH = 1.15078;
volatile float q0 = 1.0f, q1 = 0.0f, q2 = 0.0f, q3 = 0.0f; // quaternion of sensor frame relative to auxiliary frame
volatile float mahq0 = 1.0f, mahq1 = 0.0f, mahq2 = 0.0f, mahq3 = 0.0f; // quaternion of sensor frame relative to auxiliary frame
// #define sampleFreq 512.0f // sample frequency in Hz
// #define sampleFreq 25.0f // sample frequency in Hz
float sampleFreq = 20.0;
float betaDef = 0.1f; // 2 * proportional gain
void setup() {
servoX.attach(servoPinX);
servoY.attach(servoPinY);
Serial.begin(9600);
// Set to servos to level
servoAngleX = servo0X;
servoAngleY = servo0Y;
servoX.write(servoAngleX);
servoY.write(servoAngleY);
/* Initialise the sensors */
initSensors();
setupMakerPlot();
// Set the Timer 2 register values
cli(); // Disable global interrupts
TCCR2A = 0; // Set Timer Control 2 rgister to 0
TCCR2B = 0; // Set other half of Timer Control 2 register to 0
TCNT2 = 0; // Initialize counter value to 0
OCR2A = 249; // Set COMPA for Timer 2 to 249
TCCR2B |= (1 << CS22); // Start with 64 prescale
TCCR2A |= (1 << WGM21); // Turn on CTC mode
TIMSK2 |= (1 << OCIE2A); // Enable timer compare interrupt for 2A
sei(); // Enable global interrupts
}
// Timer 2 COMPA interrupt routine
// Interrupt is called once a millisecond
SIGNAL(TIMER2_COMPA_vect) {
++millisCnt;
}
void loop() {
// Check the sensor read timer
if (millisCnt >= 49 ) { // It's time to read them
readSensors();
processSensors();
millisCnt = 0;
++servoTimerCntX;
++servoTimerCntY;
}
if (servoTimerCntX >= servoTimerTestX) {
if (servoXDirectionPositive == true) {
++servoAngleX;
if (servoAngleX >= servoMaxX) servoXDirectionPositive = false;
}
else {
--servoAngleX;
if (servoAngleX <= servoMinX) servoXDirectionPositive = true;
}
servoTimerCntX = 0;
servoX.write(servoAngleX);
}
if (servoTimerCntY >= servoTimerTestY) {
if (servoYDirectionPositive == true) {
++servoAngleY;
if (servoAngleY >= servoMaxY) servoYDirectionPositive = false;
}
else {
--servoAngleY;
if (servoAngleY <= servoMinY) servoYDirectionPositive = true;
}
servoTimerCntY = 0;
servoY.write(servoAngleY);
}
printAngles();
}
void initSensors()
{
if(!accel.begin())
{
/* There was a problem detecting the LSM303 ... check your connections */
Serial.println(F("Ooops, no LSM303 detected ... Check your wiring!"));
while(1);
}
if(!mag.begin())
{
/* There was a problem detecting the LSM303 ... check your connections */
Serial.println(F("Ooops, no LSM303 detected ... Check your wiring!"));
while(1);
}
if(!bmp.begin())
{
/* There was a problem detecting the BMP180 ... check your connections */
Serial.println(F("Ooops, no BMP180 detected ... Check your wiring!"));
while(1);
}
if(!gyro.begin())
{
/* There was a problem detecting the L3GD20 ... check your connections */
Serial.print(F("Ooops, no L3GD20 detected ... Check your wiring or I2C ADDR!"));
while(1);
}
}
void readSensors() {
// Start Adafruit library specific reads
sensors_event_t event;
// Get the accelerometer data
accel.getEvent(&event);
xAccelRaw = event.acceleration.x;
yAccelRaw = event.acceleration.y;
zAccelRaw = event.acceleration.z;
// Get the gyro data
gyro.getEvent(&event);
xGyroRaw = event.gyro.x;
yGyroRaw = event.gyro.y;
zGyroRaw = event.gyro.z;
// Get the magnetometer data
mag.getEvent(&event);
xMagRaw = event.magnetic.x;
yMagRaw = event.magnetic.y;
zMagRaw = event.magnetic.z;
}
void processSensors(){
// Do the sensor calibration
xAccelCal = (xAccelRaw + xAccelOffset) * xAccelScale * accelFactor;
yAccelCal = (yAccelRaw + yAccelOffset) * yAccelScale * accelFactor;
zAccelCal = (zAccelRaw + zAccelOffset) * zAccelScale * accelFactor;
xAccelMedianArray.add(xAccelCal);
xAccelMedian = xAccelMedianArray.getMedian();
yAccelMedianArray.add(yAccelCal);
yAccelMedian = yAccelMedianArray.getMedian();
zAccelMedianArray.add(zAccelCal);
zAccelMedian = zAccelMedianArray.getMedian();
xGyroCal = (xGyroRaw + xGyroOffset) * xGyroScale;
yGyroCal = (yGyroRaw + yGyroOffset) * yGyroScale;
zGyroCal = (zGyroRaw + zGyroOffset) * zGyroScale;
xGyroMedianArray.add(xGyroCal);
xGyroMedian = xGyroMedianArray.getMedian();
yGyroMedianArray.add(yGyroCal);
yGyroMedian = yGyroMedianArray.getMedian();
zGyroMedianArray.add(zGyroCal);
zGyroMedian = zGyroMedianArray.getMedian();
xMagCal = (xMagRaw + xMagOffset) * xMagScale;
yMagCal = (yMagRaw + yMagOffset) * yMagScale;
zMagCal = (zMagRaw + zMagOffset) * zMagScale;
// Perform the calculations with the Mahoney algorithm
MahonyAHRSupdate(xGyroMedian, yGyroMedian, zGyroMedian,
xAccelMedian, yAccelMedian, zAccelMedian,
xMagCal, yMagCal, zMagCal);
mahoneyRoll = atan2(2*(mahq0*mahq1+mahq2*mahq3),(1-2*(mahq1*mahq1+mahq2*mahq2)));
mahoneyPitch = asin(2*(mahq0*mahq2-mahq3*mahq1));
mahoneyYaw = atan2(2*(mahq0*mahq3+mahq1*mahq2), 1-2*(mahq2*mahq2+mahq3*mahq3));
mahoneyRoll *= radToDegree;
mahoneyPitch *= radToDegree;
mahoneyYaw *= radToDegree;
// Perform the calculations with the Madgwick algorithm
MadgwickAHRSupdate(xGyroMedian, yGyroMedian, zGyroMedian,
xAccelMedian, yAccelMedian, zAccelMedian,
xMagCal, yMagCal, zMagCal);
madgwickRoll = atan2(2*(q0*q1+q2*q3),(1-2*(q1*q1+q2*q2)));
madgwickPitch = asin(2*(q0*q2-q3*q1));
madgwickYaw = atan2(2*(q0*q3+q1*q2), 1-2*(q2*q2+q3*q3));
madgwickRoll *= radToDegree;
madgwickPitch *= radToDegree;
madgwickYaw *= radToDegree;
// Calculate roll and pith from the accelerometer values
// accelRoll = atan2(yAccelCal, zAccelCal)*180/PI;
// intermediateValue = yAccelCal*sin(accelRoll + zAccelCal*cos(accelRoll));
// accelPitch = atan(-xAccelCal/intermediateValue*180/PI);
// unfilteredYaw = atan2(yMagCal, xMagCal) * 180 / PI;
// Another formula for roll and pitch from accelerometer
intermediateValue = sqrt(xAccelMedian * xAccelMedian + zAccelMedian*zAccelMedian);
unfilteredRoll = atan2(yAccelMedian, intermediateValue)*180/PI;
intermediateValue = sqrt(yAccelMedian * yAccelMedian + zAccelMedian*zAccelMedian);
unfilteredPitch = atan2(xAccelMedian, intermediateValue)*180/PI;
intermediateValue = xMagCal * cos(unfilteredPitch);
intermediateValue += yMagCal * sin(unfilteredPitch) * sin(unfilteredRoll);
intermediateValue += zMagCal * sin(unfilteredPitch) * cos(unfilteredRoll);
unfilteredYaw = atan2(zMagCal * sin(unfilteredRoll) - yMagCal * cos(unfilteredRoll), intermediateValue);
}
void setupMakerPlot() {
delay(100);
Serial.println(F("!O txt_Group2=Pitch")); // Label txt_Group2
Serial.println(F("!O txt_Group2=Pitch")); // Label txt_Group2
Serial.println(F("!O txt_Group1=Roll")); // Label txt_Group1
Serial.println(F("!O txt_Channel0=Madgwick Roll")); // Label txt_Channel0
Serial.println(F("!O txt_Channel1=Mahoney Roll")); // Label txt_Channel1
Serial.println(F("!O txt_Channel2=Unfiltered Roll")); // Label txt_Channel2
Serial.println(F("!O txt_Channel3=Servo X Angle")); // Label txt_Channel0
Serial.println(F("!O txt_Channel4=Not used")); // Label txt_Channel1
Serial.println(F("!O txt_Channel5=Madgwick Pitch")); // Label txt_Channel2
Serial.println(F("!O txt_Channel6=Mahoney Pitch")); // Label txt_Channel0
Serial.println(F("!O txt_Channel7=Unfiltered Pitch")); // Label txt_Channel1
Serial.println(F("!O txt_Channel8=Servo Y Angle")); // Label txt_Channel2
Serial.println(F("!O txt_Channel9=Not used")); // Label txt_Channel2
}
void printAngles() {
Serial.print(madgwickRoll);
Serial.print(",");
Serial.print(mahoneyRoll);
Serial.print(",");
Serial.print(unfilteredRoll);
Serial.print(",");
Serial.print(-(servoAngleX - servo0X));
Serial.print(",");
Serial.print(0.0, 1);
Serial.print(",");
Serial.print(-madgwickPitch);
Serial.print(",");
Serial.print(-mahoneyPitch);
Serial.print(",");
Serial.print(unfilteredPitch);
Serial.print(",");
Serial.print(servoAngleY- servo0Y);
Serial.print(",");
Serial.println(0.0, 1);
}
Code: Select all
//=====================================================================================================
// Madgwick.cpp
//=====================================================================================================
//
// Implementation of Madgwick's IMU and AHRS algorithms.
// See: http://www.x-io.co.uk/node/8#open_source_ahrs_and_imu_algorithms
//
// Date Author Notes
// 29/09/2011 SOH Madgwick Initial release
// 02/10/2011 SOH Madgwick Optimised for reduced CPU load
// 19/02/2012 SOH Madgwick Magnetometer measurement is normalised
// 03/03/2015 Walt McKinnon Converted to Arduino library
//
//=====================================================================================================
//---------------------------------------------------------------------------------------------------
// Header files
// #include "MadgwickAHRS.h"
#include <math.h>
//---------------------------------------------------------------------------------------------------
// Definitions
// #define sampleFreq 512.0f // sample frequency in Hz
// #define sampleFreq 25.0f // sample frequency in Hz
// #define betaDef 0.1f // 2 * proportional gain
//---------------------------------------------------------------------------------------------------
// Variable definitions
volatile float beta = betaDef; // 2 * proportional gain (Kp)
// volatile float q0 = 1.0f, q1 = 0.0f, q2 = 0.0f, q3 = 0.0f; // quaternion of sensor frame relative to auxiliary frame
//---------------------------------------------------------------------------------------------------
// Function declarations
float invSqrt(float x);
//====================================================================================================
// Functions
//---------------------------------------------------------------------------------------------------
void getQuaternion(float q0Out, float q1Out, float q2Out, float q3Out) {
q0Out = q0;
q1Out = q0;
q2Out = q0;
q3Out = q0;
}
// AHRS algorithm update
void MadgwickAHRSupdate(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz) {
float recipNorm;
float s0, s1, s2, s3;
float qDot1, qDot2, qDot3, qDot4;
float hx, hy;
float _2q0mx, _2q0my, _2q0mz, _2q1mx, _2bx, _2bz, _4bx, _4bz, _2q0, _2q1, _2q2, _2q3, _2q0q2, _2q2q3, q0q0, q0q1, q0q2, q0q3, q1q1, q1q2, q1q3, q2q2, q2q3, q3q3;
// Use IMU algorithm if magnetometer measurement invalid (avoids NaN in magnetometer normalisation)
if ((mx == 0.0f) && (my == 0.0f) && (mz == 0.0f)) {
MadgwickAHRSupdateIMU(gx, gy, gz, ax, ay, az);
return;
}
// Rate of change of quaternion from gyroscope
qDot1 = 0.5f * (-q1 * gx - q2 * gy - q3 * gz);
qDot2 = 0.5f * (q0 * gx + q2 * gz - q3 * gy);
qDot3 = 0.5f * (q0 * gy - q1 * gz + q3 * gx);
qDot4 = 0.5f * (q0 * gz + q1 * gy - q2 * gx);
// Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation)
if (!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) {
// Normalise accelerometer measurement
recipNorm = invSqrt(ax * ax + ay * ay + az * az);
ax *= recipNorm;
ay *= recipNorm;
az *= recipNorm;
// Normalise magnetometer measurement
recipNorm = invSqrt(mx * mx + my * my + mz * mz);
mx *= recipNorm;
my *= recipNorm;
mz *= recipNorm;
// Auxiliary variables to avoid repeated arithmetic
_2q0mx = 2.0f * q0 * mx;
_2q0my = 2.0f * q0 * my;
_2q0mz = 2.0f * q0 * mz;
_2q1mx = 2.0f * q1 * mx;
_2q0 = 2.0f * q0;
_2q1 = 2.0f * q1;
_2q2 = 2.0f * q2;
_2q3 = 2.0f * q3;
_2q0q2 = 2.0f * q0 * q2;
_2q2q3 = 2.0f * q2 * q3;
q0q0 = q0 * q0;
q0q1 = q0 * q1;
q0q2 = q0 * q2;
q0q3 = q0 * q3;
q1q1 = q1 * q1;
q1q2 = q1 * q2;
q1q3 = q1 * q3;
q2q2 = q2 * q2;
q2q3 = q2 * q3;
q3q3 = q3 * q3;
// Reference direction of Earth's magnetic field
hx = mx * q0q0 - _2q0my * q3 + _2q0mz * q2 + mx * q1q1 + _2q1 * my * q2 + _2q1 * mz * q3 - mx * q2q2 - mx * q3q3;
hy = _2q0mx * q3 + my * q0q0 - _2q0mz * q1 + _2q1mx * q2 - my * q1q1 + my * q2q2 + _2q2 * mz * q3 - my * q3q3;
_2bx = sqrt(hx * hx + hy * hy);
_2bz = -_2q0mx * q2 + _2q0my * q1 + mz * q0q0 + _2q1mx * q3 - mz * q1q1 + _2q2 * my * q3 - mz * q2q2 + mz * q3q3;
_4bx = 2.0f * _2bx;
_4bz = 2.0f * _2bz;
// Gradient decent algorithm corrective step
s0 = -_2q2 * (2.0f * q1q3 - _2q0q2 - ax) + _2q1 * (2.0f * q0q1 + _2q2q3 - ay) - _2bz * q2 * (_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) + (-_2bx * q3 + _2bz * q1) * (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) + _2bx * q2 * (_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz);
s1 = _2q3 * (2.0f * q1q3 - _2q0q2 - ax) + _2q0 * (2.0f * q0q1 + _2q2q3 - ay) - 4.0f * q1 * (1 - 2.0f * q1q1 - 2.0f * q2q2 - az) + _2bz * q3 * (_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) + (_2bx * q2 + _2bz * q0) * (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) + (_2bx * q3 - _4bz * q1) * (_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz);
s2 = -_2q0 * (2.0f * q1q3 - _2q0q2 - ax) + _2q3 * (2.0f * q0q1 + _2q2q3 - ay) - 4.0f * q2 * (1 - 2.0f * q1q1 - 2.0f * q2q2 - az) + (-_4bx * q2 - _2bz * q0) * (_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) + (_2bx * q1 + _2bz * q3) * (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) + (_2bx * q0 - _4bz * q2) * (_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz);
s3 = _2q1 * (2.0f * q1q3 - _2q0q2 - ax) + _2q2 * (2.0f * q0q1 + _2q2q3 - ay) + (-_4bx * q3 + _2bz * q1) * (_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) + (-_2bx * q0 + _2bz * q2) * (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) + _2bx * q1 * (_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz);
recipNorm = invSqrt(s0 * s0 + s1 * s1 + s2 * s2 + s3 * s3); // normalise step magnitude
s0 *= recipNorm;
s1 *= recipNorm;
s2 *= recipNorm;
s3 *= recipNorm;
// Apply feedback step
qDot1 -= beta * s0;
qDot2 -= beta * s1;
qDot3 -= beta * s2;
qDot4 -= beta * s3;
}
// Integrate rate of change of quaternion to yield quaternion
q0 += qDot1 * (1.0f / sampleFreq);
q1 += qDot2 * (1.0f / sampleFreq);
q2 += qDot3 * (1.0f / sampleFreq);
q3 += qDot4 * (1.0f / sampleFreq);
// Normalise quaternion
recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
q0 *= recipNorm;
q1 *= recipNorm;
q2 *= recipNorm;
q3 *= recipNorm;
}
//---------------------------------------------------------------------------------------------------
// IMU algorithm update
void MadgwickAHRSupdateIMU(float gx, float gy, float gz, float ax, float ay, float az) {
float recipNorm;
float s0, s1, s2, s3;
float qDot1, qDot2, qDot3, qDot4;
float _2q0, _2q1, _2q2, _2q3, _4q0, _4q1, _4q2, _8q1, _8q2, q0q0, q1q1, q2q2, q3q3;
// Rate of change of quaternion from gyroscope
qDot1 = 0.5f * (-q1 * gx - q2 * gy - q3 * gz);
qDot2 = 0.5f * (q0 * gx + q2 * gz - q3 * gy);
qDot3 = 0.5f * (q0 * gy - q1 * gz + q3 * gx);
qDot4 = 0.5f * (q0 * gz + q1 * gy - q2 * gx);
// Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation)
if (!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) {
// Normalise accelerometer measurement
recipNorm = invSqrt(ax * ax + ay * ay + az * az);
ax *= recipNorm;
ay *= recipNorm;
az *= recipNorm;
// Auxiliary variables to avoid repeated arithmetic
_2q0 = 2.0f * q0;
_2q1 = 2.0f * q1;
_2q2 = 2.0f * q2;
_2q3 = 2.0f * q3;
_4q0 = 4.0f * q0;
_4q1 = 4.0f * q1;
_4q2 = 4.0f * q2;
_8q1 = 8.0f * q1;
_8q2 = 8.0f * q2;
q0q0 = q0 * q0;
q1q1 = q1 * q1;
q2q2 = q2 * q2;
q3q3 = q3 * q3;
// Gradient decent algorithm corrective step
s0 = _4q0 * q2q2 + _2q2 * ax + _4q0 * q1q1 - _2q1 * ay;
s1 = _4q1 * q3q3 - _2q3 * ax + 4.0f * q0q0 * q1 - _2q0 * ay - _4q1 + _8q1 * q1q1 + _8q1 * q2q2 + _4q1 * az;
s2 = 4.0f * q0q0 * q2 + _2q0 * ax + _4q2 * q3q3 - _2q3 * ay - _4q2 + _8q2 * q1q1 + _8q2 * q2q2 + _4q2 * az;
s3 = 4.0f * q1q1 * q3 - _2q1 * ax + 4.0f * q2q2 * q3 - _2q2 * ay;
recipNorm = invSqrt(s0 * s0 + s1 * s1 + s2 * s2 + s3 * s3); // normalise step magnitude
s0 *= recipNorm;
s1 *= recipNorm;
s2 *= recipNorm;
s3 *= recipNorm;
// Apply feedback step
qDot1 -= beta * s0;
qDot2 -= beta * s1;
qDot3 -= beta * s2;
qDot4 -= beta * s3;
}
// Integrate rate of change of quaternion to yield quaternion
q0 += qDot1 * (1.0f / sampleFreq);
q1 += qDot2 * (1.0f / sampleFreq);
q2 += qDot3 * (1.0f / sampleFreq);
q3 += qDot4 * (1.0f / sampleFreq);
// Normalise quaternion
recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
q0 *= recipNorm;
q1 *= recipNorm;
q2 *= recipNorm;
q3 *= recipNorm;
}
//---------------------------------------------------------------------------------------------------
// Fast inverse square-root
// See: http://en.wikipedia.org/wiki/Fast_inverse_square_root
float invSqrt(float x) {
float halfx = 0.5f * x;
float y = x;
long i = *(long*)&y;
i = 0x5f3759df - (i >> 1);
y = *(float*)&i;
y = y * (1.5f - (halfx * y * y));
return y;
}
//====================================================================================================
// END OF CODE
//====================================================================================================
Code: Select all
//=====================================================================================================
// MahonyAHRS.c
//=====================================================================================================
//
// Madgwick's implementation of Mayhony's AHRS algorithm.
// See: http://www.x-io.co.uk/node/8#open_source_ahrs_and_imu_algorithms
//
// Date Author Notes
// 29/09/2011 SOH Madgwick Initial release
// 02/10/2011 SOH Madgwick Optimised for reduced CPU load
//
//=====================================================================================================
//---------------------------------------------------------------------------------------------------
// Header files
// #include "MahonyAHRS.h"
#include <math.h>
//---------------------------------------------------------------------------------------------------
// Definitions
// #define sampleFreq 512.0f // sample frequency in Hz
//#define twoKpDef (2.0f * 0.5f) // 2 * proportional gain
//#define twoKpDef (2.0f * 1.5f) // 2 * proportional gain
//#define twoKiDef (2.0f * 0.0f) // 2 * integral gain
//#define twoKpDef (2.0f * 1.5f) // 2 * proportional gain
//#define twoKiDef (2.0f * 0.1f) // 2 * integral gain
#define twoKpDef (2.0f * 1.5f) // 2 * proportional gain
#define twoKiDef (2.0f * 0.2f) // 2 * integral gain
//---------------------------------------------------------------------------------------------------
// Variable definitions
volatile float twoKp = twoKpDef; // 2 * proportional gain (Kp)
volatile float twoKi = twoKiDef; // 2 * integral gain (Ki)
// volatile float q0 = 1.0f, q1 = 0.0f, q2 = 0.0f, q3 = 0.0f; // quaternion of sensor frame relative to auxiliary frame
volatile float integralFBx = 0.0f, integralFBy = 0.0f, integralFBz = 0.0f; // integral error terms scaled by Ki
//---------------------------------------------------------------------------------------------------
// Function declarations
float invSqrtMah(float x);
//====================================================================================================
// Functions
//---------------------------------------------------------------------------------------------------
// AHRS algorithm update
void MahonyAHRSupdate(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz) {
float recipNorm;
float q0q0, q0q1, q0q2, q0q3, q1q1, q1q2, q1q3, q2q2, q2q3, q3q3;
float hx, hy, bx, bz;
float halfvx, halfvy, halfvz, halfwx, halfwy, halfwz;
float halfex, halfey, halfez;
float qa, qb, qc;
// Use IMU algorithm if magnetometer measurement invalid (avoids NaN in magnetometer normalisation)
if((mx == 0.0f) && (my == 0.0f) && (mz == 0.0f)) {
MahonyAHRSupdateIMU(gx, gy, gz, ax, ay, az);
return;
}
// Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation)
if(!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) {
// Normalise accelerometer measurement
recipNorm = invSqrtMah(ax * ax + ay * ay + az * az);
ax *= recipNorm;
ay *= recipNorm;
az *= recipNorm;
// Normalise magnetometer measurement
recipNorm = invSqrtMah(mx * mx + my * my + mz * mz);
mx *= recipNorm;
my *= recipNorm;
mz *= recipNorm;
// Auxiliary variables to avoid repeated arithmetic
q0q0 = mahq0 * mahq0;
q0q1 = mahq0 * mahq1;
q0q2 = mahq0 * mahq2;
q0q3 = mahq0 * mahq3;
q1q1 = mahq1 * mahq1;
q1q2 = mahq1 * mahq2;
q1q3 = mahq1 * mahq3;
q2q2 = mahq2 * mahq2;
q2q3 = mahq2 * mahq3;
q3q3 = mahq3 * mahq3;
// Reference direction of Earth's magnetic field
hx = 2.0f * (mx * (0.5f - q2q2 - q3q3) + my * (q1q2 - q0q3) + mz * (q1q3 + q0q2));
hy = 2.0f * (mx * (q1q2 + q0q3) + my * (0.5f - q1q1 - q3q3) + mz * (q2q3 - q0q1));
bx = sqrt(hx * hx + hy * hy);
bz = 2.0f * (mx * (q1q3 - q0q2) + my * (q2q3 + q0q1) + mz * (0.5f - q1q1 - q2q2));
// Estimated direction of gravity and magnetic field
halfvx = q1q3 - q0q2;
halfvy = q0q1 + q2q3;
halfvz = q0q0 - 0.5f + q3q3;
halfwx = bx * (0.5f - q2q2 - q3q3) + bz * (q1q3 - q0q2);
halfwy = bx * (q1q2 - q0q3) + bz * (q0q1 + q2q3);
halfwz = bx * (q0q2 + q1q3) + bz * (0.5f - q1q1 - q2q2);
// Error is sum of cross product between estimated direction and measured direction of field vectors
halfex = (ay * halfvz - az * halfvy) + (my * halfwz - mz * halfwy);
halfey = (az * halfvx - ax * halfvz) + (mz * halfwx - mx * halfwz);
halfez = (ax * halfvy - ay * halfvx) + (mx * halfwy - my * halfwx);
// Compute and apply integral feedback if enabled
if(twoKi > 0.0f) {
integralFBx += twoKi * halfex * (1.0f / sampleFreq); // integral error scaled by Ki
integralFBy += twoKi * halfey * (1.0f / sampleFreq);
integralFBz += twoKi * halfez * (1.0f / sampleFreq);
gx += integralFBx; // apply integral feedback
gy += integralFBy;
gz += integralFBz;
}
else {
integralFBx = 0.0f; // prevent integral windup
integralFBy = 0.0f;
integralFBz = 0.0f;
}
// Apply proportional feedback
gx += twoKp * halfex;
gy += twoKp * halfey;
gz += twoKp * halfez;
}
// Integrate rate of change of quaternion
gx *= (0.5f * (1.0f / sampleFreq)); // pre-multiply common factors
gy *= (0.5f * (1.0f / sampleFreq));
gz *= (0.5f * (1.0f / sampleFreq));
qa = mahq0;
qb = mahq1;
qc = mahq2;
mahq0 += (-qb * gx - qc * gy - mahq3 * gz);
mahq1 += (qa * gx + qc * gz - mahq3 * gy);
mahq2 += (qa * gy - qb * gz + mahq3 * gx);
mahq3 += (qa * gz + qb * gy - qc * gx);
// Normalise quaternion
recipNorm = invSqrtMah(mahq0 * mahq0 + mahq1 * mahq1 + mahq2 * mahq2 + mahq3 * mahq3);
mahq0 *= recipNorm;
mahq1 *= recipNorm;
mahq2 *= recipNorm;
mahq3 *= recipNorm;
}
//---------------------------------------------------------------------------------------------------
// IMU algorithm update
void MahonyAHRSupdateIMU(float gx, float gy, float gz, float ax, float ay, float az) {
float recipNorm;
float halfvx, halfvy, halfvz;
float halfex, halfey, halfez;
float qa, qb, qc;
// Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation)
if(!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) {
// Normalise accelerometer measurement
recipNorm = invSqrtMah(ax * ax + ay * ay + az * az);
ax *= recipNorm;
ay *= recipNorm;
az *= recipNorm;
// Estimated direction of gravity and vector perpendicular to magnetic flux
halfvx = mahq1 * mahq3 - mahq0 * mahq2;
halfvy = mahq0 * mahq1 + mahq2 * mahq3;
halfvz = mahq0 * mahq0 - 0.5f + mahq3 * mahq3;
// Error is sum of cross product between estimated and measured direction of gravity
halfex = (ay * halfvz - az * halfvy);
halfey = (az * halfvx - ax * halfvz);
halfez = (ax * halfvy - ay * halfvx);
// Compute and apply integral feedback if enabled
if(twoKi > 0.0f) {
integralFBx += twoKi * halfex * (1.0f / sampleFreq); // integral error scaled by Ki
integralFBy += twoKi * halfey * (1.0f / sampleFreq);
integralFBz += twoKi * halfez * (1.0f / sampleFreq);
gx += integralFBx; // apply integral feedback
gy += integralFBy;
gz += integralFBz;
}
else {
integralFBx = 0.0f; // prevent integral windup
integralFBy = 0.0f;
integralFBz = 0.0f;
}
// Apply proportional feedback
gx += twoKp * halfex;
gy += twoKp * halfey;
gz += twoKp * halfez;
}
// Integrate rate of change of quaternion
gx *= (0.5f * (1.0f / sampleFreq)); // pre-multiply common factors
gy *= (0.5f * (1.0f / sampleFreq));
gz *= (0.5f * (1.0f / sampleFreq));
qa = mahq0;
qb = mahq1;
qc = mahq2;
mahq0 += (-qb * gx - qc * gy - mahq3 * gz);
mahq1 += (qa * gx + qc * gz - mahq3 * gy);
mahq2 += (qa * gy - qb * gz + mahq3 * gx);
mahq3 += (qa * gz + qb * gy - qc * gx);
// Normalise quaternion
recipNorm = invSqrtMah(mahq0 * mahq0 + mahq1 * mahq1 + mahq2 * mahq2 + mahq3 * mahq3);
mahq0 *= recipNorm;
mahq1 *= recipNorm;
mahq2 *= recipNorm;
mahq3 *= recipNorm;
}
//---------------------------------------------------------------------------------------------------
// Fast inverse square-root
// See: http://en.wikipedia.org/wiki/Fast_inverse_square_root
float invSqrtMah(float x) {
float halfx = 0.5f * x;
float y = x;
long i = *(long*)&y;
i = 0x5f3759df - (i>>1);
y = *(float*)&i;
y = y * (1.5f - (halfx * y * y));
return y;
}
//====================================================================================================
// END OF CODE
//====================================================================================================
- Hambalala
- Posts: 9
- Joined: Sun Feb 08, 2015 2:56 pm
Re: Problem in reading for AHRS of Adafruit 10 DOF IMU
Something that I tried recently was this code below. Although it does not include the tab procedure you used, I guess that should not make a difference yet unless there are mistakes in following the coding functions. However, I seem to get not any angle at all.
Anyways, I am gonna try your one removing the unnecessary part. A question that I always had was how the sample frequency in the code makes a difference in the reading. and How different is the value that you achieve? Are you satisfied with what you get?
P.S. I really want to solve this issue for once and all.
Code: Select all
//---------------------------------------------------------------------------------------------------
// Libraries
#include <math.h>
#include <Wire.h>
#include <Adafruit_Sensor.h>
#include <Adafruit_LSM303_U.h>
#include <Adafruit_BMP085_U.h>
#include <Adafruit_L3GD20_U.h>
#include <Adafruit_10DOF.h>
#include <Adafruit_Simple_AHRS.h>
//---------------------------------------------------------------------------------------------------
// Sensor instances
Adafruit_LSM303_Accel_Unified accel(30301);
Adafruit_LSM303_Mag_Unified mag(30302);
Adafruit_BMP085_Unified bmp(18001);
Adafruit_L3GD20_Unified gyro = Adafruit_L3GD20_Unified(20);
//---------------------------------------------------------------------------------------------------
// Definitions
#define sampleFreq 25.0f // sample frequency in Hz
#define betaDef 3.0f // 2 * proportional gain
//---------------------------------------------------------------------------------------------------
// Variable definitions
volatile float beta = betaDef; // 2 * proportional gain (Kp)
volatile float q0 = 1.0f, q1 = 0.0f, q2 = 0.0f, q3 = 0.0f; // quaternion of sensor frame relative to auxiliary frame
//---------------------------------------------------------------------------------------------------
// Function declarations
float invSqrt(float x);
//---------------------------------------------------------------------------------------------------
// Fast inverse square-root
// See: http://en.wikipedia.org/wiki/Fast_inverse_square_root
float invSqrt(float x) {
float halfx = 0.5f * x;
float y = x;
long i = *(long*)&y;
i = 0x5f3759df - (i >> 1);
y = *(float*)&i;
y = y * (1.5f - (halfx * y * y));
return y;
}
int psi, theta, phi;
void setup() {
Serial.begin(9600);
Serial.println(F("Adafruit 10DOF Tester"));
Serial.println("");
accel.begin();
mag.begin();
bmp.begin();
gyro.begin();
}
void loop() {
MadgwickAHRSupdate();
psi = atan2((2 * q1 * q2) - (2 * q0 * q3), (2 * pow(q0, 2)) + (2 * pow(q1, 2)) - 1);
theta = asin((2 * q1 * q3) + (2 * q0 * q2));
phi = atan2((2 * q2 * q3) - (2 * q0 * q1), (2 * pow(q0, 2)) + (2 * pow(q3, 2)) - 1);
Serial.print(psi);
Serial.print(",");
Serial.print(theta);
Serial.print(",");
Serial.println(phi);
delay(500);
}
void MadgwickAHRSupdate() {
/* Get a new sensor event */
sensors_event_t eventg;
gyro.getEvent(&eventg);
float gx = eventg.gyro.x;
float gy = eventg.gyro.y;
float gz = eventg.gyro.z;
/* Get a new sensor event */
sensors_event_t eventa;
accel.getEvent(&eventa);
float ax = (eventa.acceleration.x)*101;
float ay = eventa.acceleration.y*101;
float az = eventa.acceleration.z*101;
/* Get a new sensor event */
sensors_event_t eventm;
mag.getEvent(&eventm);
float mx = eventm.magnetic.x;
float my = eventm.magnetic.y;
float mz = eventm.magnetic.z;
float recipNorm;
float s0, s1, s2, s3;
float qDot1, qDot2, qDot3, qDot4;
float hx, hy;
float _2q0mx, _2q0my, _2q0mz, _2q1mx, _2bx, _2bz, _4bx, _4bz, _2q0, _2q1, _2q2, _2q3, _2q0q2, _2q2q3, q0q0, q0q1, q0q2, q0q3, q1q1, q1q2, q1q3, q2q2, q2q3, q3q3;
// Use IMU algorithm if magnetometer measurement invalid (avoids NaN in magnetometer normalisation)
if ((mx == 0.0f) && (my == 0.0f) && (mz == 0.0f)) {
MadgwickAHRSupdateIMU();
return;
}
// Rate of change of quaternion from gyroscope
qDot1 = 0.5f * (-q1 * gx - q2 * gy - q3 * gz);
qDot2 = 0.5f * (q0 * gx + q2 * gz - q3 * gy);
qDot3 = 0.5f * (q0 * gy - q1 * gz + q3 * gx);
qDot4 = 0.5f * (q0 * gz + q1 * gy - q2 * gx);
// Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation)
if (!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) {
// Normalise accelerometer measurement
recipNorm = invSqrt(ax * ax + ay * ay + az * az);
ax *= recipNorm;
ay *= recipNorm;
az *= recipNorm;
// Normalise magnetometer measurement
recipNorm = invSqrt(mx * mx + my * my + mz * mz);
mx *= recipNorm;
my *= recipNorm;
mz *= recipNorm;
// Auxiliary variables to avoid repeated arithmetic
_2q0mx = 2.0f * q0 * mx;
_2q0my = 2.0f * q0 * my;
_2q0mz = 2.0f * q0 * mz;
_2q1mx = 2.0f * q1 * mx;
_2q0 = 2.0f * q0;
_2q1 = 2.0f * q1;
_2q2 = 2.0f * q2;
_2q3 = 2.0f * q3;
_2q0q2 = 2.0f * q0 * q2;
_2q2q3 = 2.0f * q2 * q3;
q0q0 = q0 * q0;
q0q1 = q0 * q1;
q0q2 = q0 * q2;
q0q3 = q0 * q3;
q1q1 = q1 * q1;
q1q2 = q1 * q2;
q1q3 = q1 * q3;
q2q2 = q2 * q2;
q2q3 = q2 * q3;
q3q3 = q3 * q3;
// Reference direction of Earth's magnetic field
hx = mx * q0q0 - _2q0my * q3 + _2q0mz * q2 + mx * q1q1 + _2q1 * my * q2 + _2q1 * mz * q3 - mx * q2q2 - mx * q3q3;
hy = _2q0mx * q3 + my * q0q0 - _2q0mz * q1 + _2q1mx * q2 - my * q1q1 + my * q2q2 + _2q2 * mz * q3 - my * q3q3;
_2bx = sqrt(hx * hx + hy * hy);
_2bz = -_2q0mx * q2 + _2q0my * q1 + mz * q0q0 + _2q1mx * q3 - mz * q1q1 + _2q2 * my * q3 - mz * q2q2 + mz * q3q3;
_4bx = 2.0f * _2bx;
_4bz = 2.0f * _2bz;
// Gradient decent algorithm corrective step
s0 = -_2q2 * (2.0f * q1q3 - _2q0q2 - ax) + _2q1 * (2.0f * q0q1 + _2q2q3 - ay) - _2bz * q2 * (_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) + (-_2bx * q3 + _2bz * q1) * (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) + _2bx * q2 * (_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz);
s1 = _2q3 * (2.0f * q1q3 - _2q0q2 - ax) + _2q0 * (2.0f * q0q1 + _2q2q3 - ay) - 4.0f * q1 * (1 - 2.0f * q1q1 - 2.0f * q2q2 - az) + _2bz * q3 * (_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) + (_2bx * q2 + _2bz * q0) * (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) + (_2bx * q3 - _4bz * q1) * (_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz);
s2 = -_2q0 * (2.0f * q1q3 - _2q0q2 - ax) + _2q3 * (2.0f * q0q1 + _2q2q3 - ay) - 4.0f * q2 * (1 - 2.0f * q1q1 - 2.0f * q2q2 - az) + (-_4bx * q2 - _2bz * q0) * (_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) + (_2bx * q1 + _2bz * q3) * (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) + (_2bx * q0 - _4bz * q2) * (_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz);
s3 = _2q1 * (2.0f * q1q3 - _2q0q2 - ax) + _2q2 * (2.0f * q0q1 + _2q2q3 - ay) + (-_4bx * q3 + _2bz * q1) * (_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) + (-_2bx * q0 + _2bz * q2) * (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) + _2bx * q1 * (_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz);
recipNorm = invSqrt(s0 * s0 + s1 * s1 + s2 * s2 + s3 * s3); // normalise step magnitude
s0 *= recipNorm;
s1 *= recipNorm;
s2 *= recipNorm;
s3 *= recipNorm;
// Apply feedback step
qDot1 -= beta * s0;
qDot2 -= beta * s1;
qDot3 -= beta * s2;
qDot4 -= beta * s3;
}
// Integrate rate of change of quaternion to yield quaternion
q0 += qDot1 * (1.0f / sampleFreq);
q1 += qDot2 * (1.0f / sampleFreq);
q2 += qDot3 * (1.0f / sampleFreq);
q3 += qDot4 * (1.0f / sampleFreq);
// Normalise quaternion
recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
q0 *= recipNorm;
q1 *= recipNorm;
q2 *= recipNorm;
q3 *= recipNorm;
}
//---------------------------------------------------------------------------------------------------
// IMU algorithm update
void MadgwickAHRSupdateIMU() {
/* Get a new sensor event */
sensors_event_t eventg;
gyro.getEvent(&eventg);
float gx = eventg.gyro.x;
float gy = eventg.gyro.y;
float gz = eventg.gyro.z;
/* Get a new sensor event */
sensors_event_t eventa;
accel.getEvent(&eventa);
float ax = eventa.acceleration.x;
float ay = eventa.acceleration.y;
float az = eventa.acceleration.z;
/* Get a new sensor event */
sensors_event_t eventm;
mag.getEvent(&eventm);
float mx = eventm.magnetic.x;
float my = eventm.magnetic.y;
float mz = eventm.magnetic.z;
float recipNorm;
float s0, s1, s2, s3;
float qDot1, qDot2, qDot3, qDot4;
float _2q0, _2q1, _2q2, _2q3, _4q0, _4q1, _4q2 , _8q1, _8q2, q0q0, q1q1, q2q2, q3q3;
// Rate of change of quaternion from gyroscope
qDot1 = 0.5f * (-q1 * gx - q2 * gy - q3 * gz);
qDot2 = 0.5f * (q0 * gx + q2 * gz - q3 * gy);
qDot3 = 0.5f * (q0 * gy - q1 * gz + q3 * gx);
qDot4 = 0.5f * (q0 * gz + q1 * gy - q2 * gx);
// Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation)
if (!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) {
// Normalise accelerometer measurement
recipNorm = invSqrt(ax * ax + ay * ay + az * az);
ax *= recipNorm;
ay *= recipNorm;
az *= recipNorm;
// Auxiliary variables to avoid repeated arithmetic
_2q0 = 2.0f * q0;
_2q1 = 2.0f * q1;
_2q2 = 2.0f * q2;
_2q3 = 2.0f * q3;
_4q0 = 4.0f * q0;
_4q1 = 4.0f * q1;
_4q2 = 4.0f * q2;
_8q1 = 8.0f * q1;
_8q2 = 8.0f * q2;
q0q0 = q0 * q0;
q1q1 = q1 * q1;
q2q2 = q2 * q2;
q3q3 = q3 * q3;
// Gradient decent algorithm corrective step
s0 = _4q0 * q2q2 + _2q2 * ax + _4q0 * q1q1 - _2q1 * ay;
s1 = _4q1 * q3q3 - _2q3 * ax + 4.0f * q0q0 * q1 - _2q0 * ay - _4q1 + _8q1 * q1q1 + _8q1 * q2q2 + _4q1 * az;
s2 = 4.0f * q0q0 * q2 + _2q0 * ax + _4q2 * q3q3 - _2q3 * ay - _4q2 + _8q2 * q1q1 + _8q2 * q2q2 + _4q2 * az;
s3 = 4.0f * q1q1 * q3 - _2q1 * ax + 4.0f * q2q2 * q3 - _2q2 * ay;
recipNorm = invSqrt(s0 * s0 + s1 * s1 + s2 * s2 + s3 * s3); // normalise step magnitude
s0 *= recipNorm;
s1 *= recipNorm;
s2 *= recipNorm;
s3 *= recipNorm;
// Apply feedback step
qDot1 -= beta * s0;
qDot2 -= beta * s1;
qDot3 -= beta * s2;
qDot4 -= beta * s3;
}
// Integrate rate of change of quaternion to yield quaternion
q0 += qDot1 * (1.0f / sampleFreq);
q1 += qDot2 * (1.0f / sampleFreq);
q2 += qDot3 * (1.0f / sampleFreq);
q3 += qDot4 * (1.0f / sampleFreq);
// Normalise quaternion
recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
q0 *= recipNorm;
q1 *= recipNorm;
q2 *= recipNorm;
q3 *= recipNorm;
}
P.S. I really want to solve this issue for once and all.
- waltmck
- Posts: 25
- Joined: Sat Jul 21, 2012 11:54 am
Re: Problem in reading for AHRS of Adafruit 10 DOF IMU
I'm really not happy with the results of the Madgwick algorithm yet. I'm not sure if its because of the sample frequency or because I don't understand the parameters enough yet. If you look at the paper for the Madgwick algorithm, it has a graph that shows it's effectiveness verses sample frequency. If I'm remembering right, you need a sample frequency of over 200 per second before you start getting out of the knee on the graph. I can't even get close to that on the Arduino. The Mahoney algorithm follows the servo angles better and does smooth out much of the noise, but I'm still not satisfied with it either. It is still too responsive to noise in the accelerometers. I suspect that this is because I'm still favoring them over the gyro too much. I haven't played with the parameters enough to see what I would need to do with them to try to favor the gyro more.
I plan on playing with this offline with the data I've collected, and even trying other custom things that I've been thing off. Like calculating based on the gyro, but resetting to the accelerometer when it looks stable. But I probably won't be doing this for awhile. I mostly want the GPS logging function and then I want to measure pitch and roll of a ship on the few times with do we a large event. I'm going to be able to do this with this code, so I've moved on to a couple of other projects. When I finish them. I'll probably move back to working on these algorithms.
I plan on playing with this offline with the data I've collected, and even trying other custom things that I've been thing off. Like calculating based on the gyro, but resetting to the accelerometer when it looks stable. But I probably won't be doing this for awhile. I mostly want the GPS logging function and then I want to measure pitch and roll of a ship on the few times with do we a large event. I'm going to be able to do this with this code, so I've moved on to a couple of other projects. When I finish them. I'll probably move back to working on these algorithms.
- Malzar
- Posts: 1
- Joined: Wed Jun 03, 2015 2:40 pm
Re: Problem in reading for AHRS of Adafruit 10 DOF IMU
I'm also having a hard time calibrating my sensor. I'm using an Arduino Uno with your Adafruit 10-DOF IMU Breakout. I'm using Unity3d to render a cube that mirrors the sensors' output. The orientation coming from the sensor seems perfect until you try to rotate the board along a single axis too much. There seems to be a pretty severe heading error.
All sensor data is sent to my computer through an Ethernet shield and is calibrated via Unity3d's native language, C#. Seeing as how the above referenced URL includes Madgwick's code written in C#, I just grabbed a copy of that.
Just a heads up as to why the axes seem strangely ordered in my code...
Traditionally...
x = roll
y = pitch
z = heading
Unity3d...
x = pitch
y = heading
z = roll
MadgwickAHRS.cs
Code used to render the cube in Unity3d...
Video of results...
Left Cube: Uncalibrated. (heading error)
Right Cube: Madgwick calibrated. (broken)
Granted, I'm rotating it by hand, but the heading error is a little more obvious at the end of the video when I rotate it around 180 degrees.
https://youtu.be/eqRDyFNqr3U
Anyone ever figure out how to implement Madgwick's code properly?
All sensor data is sent to my computer through an Ethernet shield and is calibrated via Unity3d's native language, C#. Seeing as how the above referenced URL includes Madgwick's code written in C#, I just grabbed a copy of that.
Just a heads up as to why the axes seem strangely ordered in my code...
Traditionally...
x = roll
y = pitch
z = heading
Unity3d...
x = pitch
y = heading
z = roll
MadgwickAHRS.cs
Code: Select all
using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
namespace AHRS
{
/// <summary>
/// MadgwickAHRS class. Implementation of Madgwick's IMU and AHRS algorithms.
/// </summary>
/// <remarks>
/// See: http://www.x-io.co.uk/node/8#open_source_ahrs_and_imu_algorithms
/// </remarks>
public class MadgwickAHRS
{
/// <summary>
/// Gets or sets the sample period.
/// </summary>
public float SamplePeriod { get; set; }
/// <summary>
/// Gets or sets the algorithm gain beta.
/// </summary>
public float Beta { get; set; }
/// <summary>
/// Gets or sets the Quaternion output.
/// </summary>
public float[] Quaternion { get; set; }
/// <summary>
/// Initializes a new instance of the <see cref="MadgwickAHRS"/> class.
/// </summary>
/// <param name="samplePeriod">
/// Sample period.
/// </param>
public MadgwickAHRS(float samplePeriod)
: this(samplePeriod, 1f)
{
}
/// <summary>
/// Initializes a new instance of the <see cref="MadgwickAHRS"/> class.
/// </summary>
/// <param name="samplePeriod">
/// Sample period.
/// </param>
/// <param name="beta">
/// Algorithm gain beta.
/// </param>
public MadgwickAHRS(float samplePeriod, float beta)
{
SamplePeriod = samplePeriod;
Beta = beta;
Quaternion = new float[] { 1f, 0f, 0f, 0f };
}
/// <summary>
/// Algorithm AHRS update method. Requires only gyroscope and accelerometer data.
/// </summary>
/// <param name="gx">
/// Gyroscope x axis measurement in radians/s.
/// </param>
/// <param name="gy">
/// Gyroscope y axis measurement in radians/s.
/// </param>
/// <param name="gz">
/// Gyroscope z axis measurement in radians/s.
/// </param>
/// <param name="ax">
/// Accelerometer x axis measurement in any calibrated units.
/// </param>
/// <param name="ay">
/// Accelerometer y axis measurement in any calibrated units.
/// </param>
/// <param name="az">
/// Accelerometer z axis measurement in any calibrated units.
/// </param>
/// <param name="mx">
/// Magnetometer x axis measurement in any calibrated units.
/// </param>
/// <param name="my">
/// Magnetometer y axis measurement in any calibrated units.
/// </param>
/// <param name="mz">
/// Magnetometer z axis measurement in any calibrated units.
/// </param>
/// <remarks>
/// Optimised for minimal arithmetic.
/// Total ±: 160
/// Total *: 172
/// Total /: 5
/// Total sqrt: 5
/// </remarks>
public void Update(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz)
{
float q1 = Quaternion[0], q2 = Quaternion[1], q3 = Quaternion[2], q4 = Quaternion[3]; // short name local variable for readability
float norm;
float hx, hy, _2bx, _2bz;
float s1, s2, s3, s4;
float qDot1, qDot2, qDot3, qDot4;
// Auxiliary variables to avoid repeated arithmetic
float _2q1mx;
float _2q1my;
float _2q1mz;
float _2q2mx;
float _4bx;
float _4bz;
float _2q1 = 2f * q1;
float _2q2 = 2f * q2;
float _2q3 = 2f * q3;
float _2q4 = 2f * q4;
float _2q1q3 = 2f * q1 * q3;
float _2q3q4 = 2f * q3 * q4;
float q1q1 = q1 * q1;
float q1q2 = q1 * q2;
float q1q3 = q1 * q3;
float q1q4 = q1 * q4;
float q2q2 = q2 * q2;
float q2q3 = q2 * q3;
float q2q4 = q2 * q4;
float q3q3 = q3 * q3;
float q3q4 = q3 * q4;
float q4q4 = q4 * q4;
// Normalise accelerometer measurement
norm = (float)Math.Sqrt(ax * ax + ay * ay + az * az);
if (norm == 0f) return; // handle NaN
norm = 1 / norm; // use reciprocal for division
ax *= norm;
ay *= norm;
az *= norm;
// Normalise magnetometer measurement
norm = (float)Math.Sqrt(mx * mx + my * my + mz * mz);
if (norm == 0f) return; // handle NaN
norm = 1 / norm; // use reciprocal for division
mx *= norm;
my *= norm;
mz *= norm;
// Reference direction of Earth's magnetic field
_2q1mx = 2f * q1 * mx;
_2q1my = 2f * q1 * my;
_2q1mz = 2f * q1 * mz;
_2q2mx = 2f * q2 * mx;
hx = mx * q1q1 - _2q1my * q4 + _2q1mz * q3 + mx * q2q2 + _2q2 * my * q3 + _2q2 * mz * q4 - mx * q3q3 - mx * q4q4;
hy = _2q1mx * q4 + my * q1q1 - _2q1mz * q2 + _2q2mx * q3 - my * q2q2 + my * q3q3 + _2q3 * mz * q4 - my * q4q4;
_2bx = (float)Math.Sqrt(hx * hx + hy * hy);
_2bz = -_2q1mx * q3 + _2q1my * q2 + mz * q1q1 + _2q2mx * q4 - mz * q2q2 + _2q3 * my * q4 - mz * q3q3 + mz * q4q4;
_4bx = 2f * _2bx;
_4bz = 2f * _2bz;
// Gradient decent algorithm corrective step
s1 = -_2q3 * (2f * q2q4 - _2q1q3 - ax) + _2q2 * (2f * q1q2 + _2q3q4 - ay) - _2bz * q3 * (_2bx * (0.5f - q3q3 - q4q4) + _2bz * (q2q4 - q1q3) - mx) + (-_2bx * q4 + _2bz * q2) * (_2bx * (q2q3 - q1q4) + _2bz * (q1q2 + q3q4) - my) + _2bx * q3 * (_2bx * (q1q3 + q2q4) + _2bz * (0.5f - q2q2 - q3q3) - mz);
s2 = _2q4 * (2f * q2q4 - _2q1q3 - ax) + _2q1 * (2f * q1q2 + _2q3q4 - ay) - 4f * q2 * (1 - 2f * q2q2 - 2f * q3q3 - az) + _2bz * q4 * (_2bx * (0.5f - q3q3 - q4q4) + _2bz * (q2q4 - q1q3) - mx) + (_2bx * q3 + _2bz * q1) * (_2bx * (q2q3 - q1q4) + _2bz * (q1q2 + q3q4) - my) + (_2bx * q4 - _4bz * q2) * (_2bx * (q1q3 + q2q4) + _2bz * (0.5f - q2q2 - q3q3) - mz);
s3 = -_2q1 * (2f * q2q4 - _2q1q3 - ax) + _2q4 * (2f * q1q2 + _2q3q4 - ay) - 4f * q3 * (1 - 2f * q2q2 - 2f * q3q3 - az) + (-_4bx * q3 - _2bz * q1) * (_2bx * (0.5f - q3q3 - q4q4) + _2bz * (q2q4 - q1q3) - mx) + (_2bx * q2 + _2bz * q4) * (_2bx * (q2q3 - q1q4) + _2bz * (q1q2 + q3q4) - my) + (_2bx * q1 - _4bz * q3) * (_2bx * (q1q3 + q2q4) + _2bz * (0.5f - q2q2 - q3q3) - mz);
s4 = _2q2 * (2f * q2q4 - _2q1q3 - ax) + _2q3 * (2f * q1q2 + _2q3q4 - ay) + (-_4bx * q4 + _2bz * q2) * (_2bx * (0.5f - q3q3 - q4q4) + _2bz * (q2q4 - q1q3) - mx) + (-_2bx * q1 + _2bz * q3) * (_2bx * (q2q3 - q1q4) + _2bz * (q1q2 + q3q4) - my) + _2bx * q2 * (_2bx * (q1q3 + q2q4) + _2bz * (0.5f - q2q2 - q3q3) - mz);
norm = 1f / (float)Math.Sqrt(s1 * s1 + s2 * s2 + s3 * s3 + s4 * s4); // normalise step magnitude
s1 *= norm;
s2 *= norm;
s3 *= norm;
s4 *= norm;
// Compute rate of change of quaternion
qDot1 = 0.5f * (-q2 * gx - q3 * gy - q4 * gz) - Beta * s1;
qDot2 = 0.5f * (q1 * gx + q3 * gz - q4 * gy) - Beta * s2;
qDot3 = 0.5f * (q1 * gy - q2 * gz + q4 * gx) - Beta * s3;
qDot4 = 0.5f * (q1 * gz + q2 * gy - q3 * gx) - Beta * s4;
// Integrate to yield quaternion
q1 += qDot1 * SamplePeriod;
q2 += qDot2 * SamplePeriod;
q3 += qDot3 * SamplePeriod;
q4 += qDot4 * SamplePeriod;
norm = 1f / (float)Math.Sqrt(q1 * q1 + q2 * q2 + q3 * q3 + q4 * q4); // normalise quaternion
Quaternion[0] = q1 * norm;
Quaternion[1] = q2 * norm;
Quaternion[2] = q3 * norm;
Quaternion[3] = q4 * norm;
}
/// <summary>
/// Algorithm IMU update method. Requires only gyroscope and accelerometer data.
/// </summary>
/// <param name="gx">
/// Gyroscope x axis measurement in radians/s.
/// </param>
/// <param name="gy">
/// Gyroscope y axis measurement in radians/s.
/// </param>
/// <param name="gz">
/// Gyroscope z axis measurement in radians/s.
/// </param>
/// <param name="ax">
/// Accelerometer x axis measurement in any calibrated units.
/// </param>
/// <param name="ay">
/// Accelerometer y axis measurement in any calibrated units.
/// </param>
/// <param name="az">
/// Accelerometer z axis measurement in any calibrated units.
/// </param>
/// <remarks>
/// Optimised for minimal arithmetic.
/// Total ±: 45
/// Total *: 85
/// Total /: 3
/// Total sqrt: 3
/// </remarks>
public void Update(float gx, float gy, float gz, float ax, float ay, float az)
{
float q1 = Quaternion[0], q2 = Quaternion[1], q3 = Quaternion[2], q4 = Quaternion[3]; // short name local variable for readability
float norm;
float s1, s2, s3, s4;
float qDot1, qDot2, qDot3, qDot4;
// Auxiliary variables to avoid repeated arithmetic
float _2q1 = 2f * q1;
float _2q2 = 2f * q2;
float _2q3 = 2f * q3;
float _2q4 = 2f * q4;
float _4q1 = 4f * q1;
float _4q2 = 4f * q2;
float _4q3 = 4f * q3;
float _8q2 = 8f * q2;
float _8q3 = 8f * q3;
float q1q1 = q1 * q1;
float q2q2 = q2 * q2;
float q3q3 = q3 * q3;
float q4q4 = q4 * q4;
// Normalise accelerometer measurement
norm = (float)Math.Sqrt(ax * ax + ay * ay + az * az);
if (norm == 0f) return; // handle NaN
norm = 1 / norm; // use reciprocal for division
ax *= norm;
ay *= norm;
az *= norm;
// Gradient decent algorithm corrective step
s1 = _4q1 * q3q3 + _2q3 * ax + _4q1 * q2q2 - _2q2 * ay;
s2 = _4q2 * q4q4 - _2q4 * ax + 4f * q1q1 * q2 - _2q1 * ay - _4q2 + _8q2 * q2q2 + _8q2 * q3q3 + _4q2 * az;
s3 = 4f * q1q1 * q3 + _2q1 * ax + _4q3 * q4q4 - _2q4 * ay - _4q3 + _8q3 * q2q2 + _8q3 * q3q3 + _4q3 * az;
s4 = 4f * q2q2 * q4 - _2q2 * ax + 4f * q3q3 * q4 - _2q3 * ay;
norm = 1f / (float)Math.Sqrt(s1 * s1 + s2 * s2 + s3 * s3 + s4 * s4); // normalise step magnitude
s1 *= norm;
s2 *= norm;
s3 *= norm;
s4 *= norm;
// Compute rate of change of quaternion
qDot1 = 0.5f * (-q2 * gx - q3 * gy - q4 * gz) - Beta * s1;
qDot2 = 0.5f * (q1 * gx + q3 * gz - q4 * gy) - Beta * s2;
qDot3 = 0.5f * (q1 * gy - q2 * gz + q4 * gx) - Beta * s3;
qDot4 = 0.5f * (q1 * gz + q2 * gy - q3 * gx) - Beta * s4;
// Integrate to yield quaternion
q1 += qDot1 * SamplePeriod;
q2 += qDot2 * SamplePeriod;
q3 += qDot3 * SamplePeriod;
q4 += qDot4 * SamplePeriod;
norm = 1f / (float)Math.Sqrt(q1 * q1 + q2 * q2 + q3 * q3 + q4 * q4); // normalise quaternion
Quaternion[0] = q1 * norm;
Quaternion[1] = q2 * norm;
Quaternion[2] = q3 * norm;
Quaternion[3] = q4 * norm;
}
}
}
Code: Select all
using UnityEngine;
using System.Collections;
using System;
public class SensorExample : MonoBehaviour
{
AHRS.MadgwickAHRS calibration;
Transform leftCube;
Orientation orientation;
Acceleration acceleration;
Torque torque;
void Start ()
{
calibration = new AHRS.MadgwickAHRS (25.0f);
}
void Update ()
{
//-------------------------------------------------------------------
//CODE GOES HERE THAT UPDATES TORQUE(GYRO), ORIENTATION, AND ACCELERATION.
//-------------------------------------------------------------------
//Left cube reflects raw data.
leftCube.eulerAngles = new Vector3(-orientation.pitch, -orientation.heading, -orientation.roll);
calibration.Update(-deg2Rad(torque.x),
-deg2Rad(torque.y),
-deg2Rad(torque.z),
-m2G(acceleration.x),
-m2G(acceleration.y),
-m2G(acceleration.z),
-orientation.pitch,
-orientation.heading,
-orientation.roll);
float[] quat = calibration.Quaternion;
this.transform.rotation = new Quaternion(quat[0], quat[1], quat[2], quat[3]);
}
//meters per second squared conversion to milli g-units
public float m2G(float m)
{
//1 m/s^2 = 0.10197162129779 g-units
//1 g-unit = 1000 millig-units
return (m * 0.10197162129779f) * 1000;
}
//degrees to radians conversion...
public float deg2Rad(float d)
{
return (float)(d * Math.PI) / 180.0f;
}
public struct Torque
{
public float x;
public float y;
public float z;
public Torque(float x, float y, float z)
{
this.x = x;
this.y = y;
this.z = z;
}
}
public struct Acceleration
{
public float x;
public float y;
public float z;
public Acceleration(float x, float y, float z)
{
this.x = x;
this.y = y;
this.z = z;
}
}
public struct Orientation
{
public float roll;
public float pitch;
public float heading;
public Orientation(float roll, float pitch, float heading)
{
this.roll = roll;
this.pitch = pitch;
this.heading = heading;
}
}
}
Left Cube: Uncalibrated. (heading error)
Right Cube: Madgwick calibrated. (broken)
Granted, I'm rotating it by hand, but the heading error is a little more obvious at the end of the video when I rotate it around 180 degrees.
https://youtu.be/eqRDyFNqr3U
Anyone ever figure out how to implement Madgwick's code properly?
Please be positive and constructive with your questions and comments.