search

kriswiner / MPU9250

Arduino sketches for MPU9250 9DoF with AHRS sensor fusion
1.04k stars 471 forks source link

Yaw Pitch Roll Estimation #41

Open akhilmohan4487 opened 8 years ago

akhilmohan4487 commented 8 years ago

Hello Kris, Hope you are doing good. I am having some problems while estimating YPR. I am reading data from FIFO at 1kHz with BW 184 Hz and then magneto at 100Hz (i hope 0x12 and 0x16 setting on CNTL does that job). Now I have got the scale and offset separately via offline and is being continuously subtracted and scaled for accelero, gyro and magneto. Now this one time calibrated data is sent to Madgwick filter. I am not sure the order to be sent. Madwick seems to use NED but the code that you shared does not follow that. It is in NWUp for accelro/gyro and NWDown for magneto. I am not sure how this could be done. After this I have the quat -> rotmatrix conversion and then calculation of euler angle using ZXY sequence. The angle that I calculate is not linearly incrementing but at some angle it is stationary and at some small angle change there is a large variation. Can you give me some idea to solve this issue ? Thanks and Regards, Akhil

kriswiner commented 8 years ago

Sounds like your mag is not properly calibrated. If you plot Mx vs Mz gathered while moving the magnetometer in a figure eight, do you get a spherical cloud of points?

akhilmohan4487 commented 8 years ago

Magneto XZY data.txt Hello, I have attached the magnetometer data in XZY sequence after one time calibration. Can you take a look and tell me what is wrong, I am not able to plot the data in a convenient way.

Thanks and Regards, Akhil

kriswiner commented 8 years ago

Try excel.

-----Original Message----- From: Akhil Mohan [mailto:notifications@github.com] Sent: February 11, 2016 2:01 AM To: kriswiner/MPU-9250 Cc: Kris Winer Subject: Re: [MPU-9250] Yaw Pitch Roll Estimation (#41)

Magneto XZY data.txt https://github.com/kriswiner/MPU-9250/files/126151/Magneto.XZY.data.txt Hello, I have attached the magnetometer data in XZY sequence after one time calibration. Can you take a look and tell me what is wrong, I am not able to plot the data in a convenient way.

Thanks and Regards, Akhil

Reply to this email directly or view it on GitHub https://github.com/kriswiner/MPU-9250/issues/41#issuecomment-182788543 . https://github.com/notifications/beacon/AGY1qlIl4PTIrB5GkXysJltddkx1Ydyoks5 pjFNUgaJpZM4HXnV3.gif

akhilmohan4487 commented 8 years ago

calibrated mag result

calibrated mag result2

calibrated mag result3

I am not able to interpret the meaning fully, its not centered wrt origin. What is your opinion on the result ?

kriswiner commented 8 years ago

Was this data taken while you were moving the sensor in a figure eight motion to sample the entire response surface?

If so it shows a very biased magnetometer response. You need to calculate the offsets and apply them to the subsequent data in order to recenter the response surface on the origin.

There is no way you are going to get sensible results with this kind of poor calibration.

See: https://github.com/kriswiner/MPU-6050/wiki/Simple-and-Effective-Magnetometer -Calibration

-----Original Message----- From: Akhil Mohan [mailto:notifications@github.com] Sent: February 12, 2016 1:45 AM To: kriswiner/MPU-9250 Cc: Kris Winer Subject: Re: [MPU-9250] Yaw Pitch Roll Estimation (#41)

calibrated mag result https://cloud.githubusercontent.com/assets/5743851/13003359/33892b3c-d19b-1 1e5-9161-f6a3a35f2523.png I am not able to interpret the meaning fully, its not centered wrt origin. What is your opinion on the result ?

Reply to this email directly or view it on GitHub https://github.com/kriswiner/MPU-9250/issues/41#issuecomment-183254547 . https://github.com/notifications/beacon/AGY1qvPoTtOjOo26xwjs9T4yPtkGqWcOks5 pjaEigaJpZM4HXnV3.gif

akhilmohan4487 commented 8 years ago

Yes, the data is taken while movement as you suggested. I followed the wiki article you suggested and have already implemented. Instead of each time calibrating. I have taken data once and cakculated the hard iron and soft iron biases. The hard irn is then converted to mG and is being constantly subtracted. Thisbis then multiplied by the soft irin scale factor. The plot obtained is after akl this work. So what do you think the mistake that i have done?

akhilmohan4487 commented 8 years ago

Sorry lots of typo error,am using phone.

kriswiner commented 8 years ago

I don't know what the problem is. Once you have the biases, you should be able to subtract them from the subsequent data and get cloud of points centered at the origin. Sounds like either the scaling or subtraction is wrong.

-----Original Message----- From: Akhil Mohan [mailto:notifications@github.com] Sent: February 12, 2016 9:16 AM To: kriswiner/MPU-9250 Cc: Kris Winer Subject: Re: [MPU-9250] Yaw Pitch Roll Estimation (#41)

Sorry lots of typo error,am using phone.

Reply to this email directly or view it on GitHub https://github.com/kriswiner/MPU-9250/issues/41#issuecomment-183418015 . https://github.com/notifications/beacon/AGY1qtpNzk30DIkbJ6Ox8q3aKXhA_G13ks5 pjgrBgaJpZM4HXnV3.gif

akhilmohan4487 commented 8 years ago

I have recorded the data once moving in some possible combinations covering the 3D space and then obtained the biases. Each time when a magneto read a data, the biases are removed (one time calibration). But still my estimation of YPR is not correct. The sequence right now to the filter is, Filter (ax,ay,az,gx,gy,gx,my,mx,mz) where gyro are in rad/s. I feel it should be my,-mx and mz but that also is not giving me a good result. The quat is converted to rotation matrix and then used to determine Y, P and R. I am not sure what mistake I am making now. magneto_calib_1 magneto_calib_2 magneto_calib_3

kriswiner commented 8 years ago

Hi Akhil,

It looks like you have the mag more or less properly calibrated.

You can read about the use of the North-East-Down convention here:

https://github.com/kriswiner/EM7180_SENtral_sensor_hub/wiki/C.-Managing-the- Configuration-File

which could help you choose the proper order to pass the sensor data to the Madgwick algorithm.

Additionally, this is how I go from quaternions to YPR:

Heading = atan2[(Qx2 - Qy2 - Qz2 + Qw2), 2(QxQy + QzQw)] Pitch = asin[-2(QxQz - QyQw)] Roll = atan2[(-Qx2 - Qy2 + Qz2 + Qw2), 2*(QxQw + QyQz)]

Even with everything correct you shouldn't expect heading accuracy much better than four or five degrees with this simple algorithm.

Kris

-----Original Message----- From: Akhil Mohan [mailto:notifications@github.com] Sent: February 17, 2016 12:38 AM To: kriswiner/MPU-9250 Cc: Kris Winer Subject: Re: [MPU-9250] Yaw Pitch Roll Estimation (#41)

I have recorded the data once moving in some possible combinations covering the 3D space and then obtained the biases. Each time when a magneto read a data, the biases are removed (one time calibration). But still my estimation of YPR is not correct. The sequence right now to the filter is, Filter (ax,ay,az,gx,gy,gx,my,mx,mz) where gyro are in rad/s. I feel it should be my,-mx and mz but that also is not giving me a good result. The quat is converted to rotation matrix and then used to determine Y, P and R. I am not sure what mistake I am making now. magneto_calib_1 https://cloud.githubusercontent.com/assets/5743851/13103917/e2423f32-d57f-1 1e5-8823-01a281460834.png magneto_calib_2 https://cloud.githubusercontent.com/assets/5743851/13103918/e24615d0-d57f-1 1e5-8648-1a759ae56195.png magneto_calib_3 https://cloud.githubusercontent.com/assets/5743851/13103919/e26e8380-d57f-1 1e5-8f84-4067e7201c50.png

Reply to this email directly or view it on GitHub https://github.com/kriswiner/MPU-9250/issues/41#issuecomment-185095192 . https://github.com/notifications/beacon/AGY1qoQgRT-zdlJ5cwyLNIDqLuV2p5ndks5 plCkCgaJpZM4HXnV3.gif