then applying corection matrix together with biases on raw data - graph looks good to me.
Now i want to ensure that its working as expected.
Moving flat with sensor around z axis and applying corection matrix and bias.
Is magnetometer correctly calibrated?
I have doubts coz when using AHRS filter i am getting wrong yaw values while rotating sensor. For ex. when rotating around z-axis 90 degrees, yaw changes only by 60 degrees.
The goal is to get 3-axis absolute orientation but thats imposible without properly calibrated magnetometer.
Note: if the magnetometer is properly calibrated for 3D, the points in the (X,Y) projection will not be centered on the plot origin, because the Earth's magnetic field is not perpendicular to the ground (except at the magnetic poles).
When rotating 90 degree left while imu is laying flat on the table (around z-axis) i measure 115 degrees delta, and when rotating to right 90 degree imu measures 60 degrees. In short story, true and measured values do not match.
If the IMU is properly calibrated, yet giving incorrect readings, then it is likely that magnetic materials or current carrying wires are nearby. Or you are using nonfunctional code -- the Sparkfun code produces nonsensical angles.
The IMU has to be calibrated in place to correct for environmental influences.
Aaa what distance is nearby? Tried to move out of my desk. I have only connected arduino thought usb to laptop which is about one meter far from my imu. Biases computed were different (shrunk) but problem still occurs. Computed pitch and roll absolute orientation is good, but yaw still not...
Just wondering if it was not better to devide problem into smaller pieces and try to make compass first? To better see what is going wrong.
I'm just thinking out loud - when rotating flat on table - only x, y uMag_T coordinates should change and i should be able to compute heading (to make compass), correct me if i am wrong.
when rotating flat on table - only x, y uMag_T coordinates should change
No, magnetometer x,y,z values all change. The magnetic field does not point straight into or out of the ground.
You can make a compass by calibrating the x and y values to form a circle centered on the origin, but it will only work if the compass is held level.
For that reason, people use the accelerometer for tilt compensation, but the magnetometer has to be calibrated in 3D for that to work. A tilt compensated compass example for the MPU-9250 is on my Github page.
Ok, coded tilt compensated compass (based on your Github example).
I calibrated it at least ten times and still getting 30 degrees accuracy. Also i went into suitable environment and calibrated there. Noticed that in calibrating data there are outliers, so i removed them. Getting this ugly result.
Then run magneto 1.2
And getting quite good sphere, but why it is not working then?
Apply corrections and substracting bias but still same bad accuracy.
Maybe magnetometer is defective...
Code works - i used magnetometer data from my iPhone and check some angles and they matched with 2 degrees accuracy.
EDIT2: Maybe i found the problem, i think my cables connected to the MPU are affecting magnetometer..., I probably will need to place mpu in some case and make wiring behave as a hard iron distortion.
Here are another calibration data:
What scares me little bit is a left top corner showing multilayered elipsoid.
Multilayer elispoid was caused by wiring distortion, i solved it by fixing imu onto arduino and then getting calibration data again.
Accuracy got better. Also When getting calibration data i rounded data to one decimal place. But it looks thats not anough. Still getting 15 degrees accuracy.
Computed bias from magneto is always calculated similarly - that is not changing so much now. Also notice that transormation matrix is has diagonal near "1" and offs near zero - it looks like there is no/small missallignment and soft iron.
#include "MPU9250.h"
#include <ESP8266WiFi.h>
int imuu2MemoryStartAddr = 12;
long long counter = 0;
MPU9250 imuu2(SPI, 4, imuu2MemoryStartAddr);
int status;
void initSensor(MPU9250 &imu, int i) {
status = imu.begin();
if (status < 0) {
Serial.println((String) "IMU initialization unsuccessful, num " + i);
Serial.println((String) "Status: " + status);
while (1) {}
}
Serial.println("Sensor initialized");
}
void vector_cross(float a[3], float b[3], float out[3])
{
out[0] = a[1] * b[2] - a[2] * b[1];
out[1] = a[2] * b[0] - a[0] * b[2];
out[2] = a[0] * b[1] - a[1] * b[0];
}
float vector_dot(float a[3], float b[3])
{
return a[0] * b[0] + a[1] * b[1] + a[2] * b[2];
}
void vector_normalize(float a[3])
{
float mag = sqrt(vector_dot(a, a));
a[0] /= mag;
a[1] /= mag;
a[2] /= mag;
}
// Returns a heading (in degrees) given an acceleration vector a due to gravity, a magnetic vector m, and a facing vector p.
int get_heading(float acc[3], float mag[3], float p[3])
{
float E[3], N[3]; //direction vectors
// cross "down" (acceleration vector) with magnetic vector (magnetic north + inclination) with to produce "east"
vector_cross(acc, mag, E);
vector_normalize(E);
// cross "east" with "down" to produce "north" (parallel to the ground)
vector_cross(E, acc, N);
vector_normalize(N);
// compute heading
int heading = round(atan2(vector_dot(E, p), vector_dot(N, p)) * 180 / M_PI);
heading = -heading; //conventional nav, heading increases North to East
if (heading < 0)
heading += 360;
return heading;
}
float p[] = {1, 0, 0};
void readSensor(MPU9250 & imu, int i) {
float gx, gy, gz, ax, ay, az, mx, my, mz, pitch, roll, yaw, deltat, mbx, mby, mbz;
float tm[3][3] = {
{ +1.04317, +0.00641, +0.00776 },
{ +0.00641, +1.00407, -0.06489 },
{ +0.00776, -0.06489, +1.07991 }
};
mbx = -15.84, mby = -6.18, mbz = -5.07;
imu.readSensor();
ax = imu.getAccelX_mss(), ay = imu.getAccelY_mss(), az = imu.getAccelZ_mss();
gx = imu.getGyroX_rads(), gy = imu.getGyroY_rads(), gz = imu.getGyroZ_rads();
mx = imu.getMagX_uT(), my = imu.getMagY_uT(), mz = imu.getMagZ_uT();
mx = mx - mbx, my = my - mby, mz = mz - mbz;
float smx = (mx * tm[0][0]) + (my * tm[0][1]) + (mz * tm[0][2]);
float smy = (mx * tm[1][0]) + (my * tm[1][1]) + (mz * tm[1][2]);
float smz = (mx * tm[2][0]) + (my * tm[2][1]) + (mz * tm[2][2]);
if (counter % 1000 == 0) {
float a = sqrtf(ax * ax + ay * ay + az * az);
ax /= a;
ay /= a;
az /= a;
float h = sqrtf(smx * smx + smy * smy + smz * smz);
smx /= h;
smy /= h;
smz /= h;
az = -az; //invert z axis
float Axyz[3] = {ay, ax, az};
float Mxyz[3] = {smx, smy, smz};
Serial.println(get_heading(Axyz, Mxyz, p));
}
}
float constrainAngle360(float dta) {
dta = fmod(dta, 2.0 * PI);
if (dta < 0.0)
dta += 2.0 * PI;
return dta;
}
void setup() {
Serial.begin(115200);
initSensor(imuu2, 1);
imuu2.loadAccelCalFromMemory();
}
void loop() {
counter++;
readSensor(imuu2, 0);
}
