GyroLib is a free, open-source Attitude and Heading Reference System (AHRS) library for the Matlab.
It includes several basic algorithms that allow to determine the orientation of the device equipped with the accelerometers and gyroscopes and also with vector magnetometer:
- TRIAD algorithm;
- QUEST algorithm;
- AHRS algorithm with complementary Kalman Filter.
Recordings of test data from MEMS IMUs are provided to asses algorithm performance in real-life situations.
Vlad Maximov (2020). GyroLib - AHRS Library (https://www.mathworks.com/matlabcentral/fileexchange/63250-gyrolib-ahrs-library), MATLAB Central File Exchange. Retrieved .
I was wondering the same question as Milad previously?
If you could please clear this confusion. Does dWb reflect raw gyro measurement or an integral of gyro values?
thanks for sharing these great codes.
I have a simple question: I have some real IMU data, and I was wondering what is the meaning of
"dwb - Integral of angular rate vector over the computer cycle" for "ahrs_quat" function?
is it the raw output of the gyro (rad/s) or gyro*dt or perhaps something else?
thanks for your time
Hi Terence, thanks for pointing that H bug out, I'll post an update some time soon :)
Hi Vlad, thanks for your response - implemented and seems good - it actually just led me to pick up one potential bug in the AHRS_quat code: in evaluating S on line 92, the second H isn't transposed - probably wasn't picked up because they are square matrices. AHRS_dcm doesn't have this issue.
To answer your question I have magnetometers with different characteristics and I want the Kalman filter to use the most appropriate magnetometer given the current dynamics. Many thanks again.
You're welcome. It's really easy to modify the code, H will get 3 additional rows, and R will get 3 new members on its diagonal. A, F, G, Qn won't change at all, as they describe error dynamics for gyroscopes, and in this simple implementation they don't depend in any way on the measurement model. If you, for example, wanted to add magnetometer and accelerometers errors to the state vector then those matrices will obviously change. BTW, why do you need an additional magnetometer? We've got only one magnetic field vector, both magnetometers will measure the same thing...
Hi Vlad, thank you for this excellent and clear library. Do you have any suggestions on modifying it for adding an additional 3-axis magnetometer, for a total of 3 measurement inputs? Obviously the measurement matrices H and R are now 9x9 with the additional measurement, but how would A, F, G and Qn change?
Sure, just comment out sections (4:6) of measurement vector v and matrix H related to the "magnetometer" measurements. Strictly there's no "magnetic field" and "gravity" vectors there - just two non-collinear vectors. If you leave only one vector - say, the "gravity", that points down, you'll keep the ability to correct the horizontal components of the attitude, but will loose the heading correction.
Can this code or it's inputs (specifically Mb and mn) be modified to handle IMUs without a magnetometer?
Hello, Thanks for your great efforts to make this function.
I need to know how can I determine these two parameters:
% fb - Acceleration vector in body frame [3x1]
% mb - Magnetic field vector in body frame [3x1]
% fn - Gravity vector in navigation frame [3x1]
% mn - Magetic field vector in navigation frame [3x1]
My sensor gust generate acceleration [ax,ay,az], gyroscope [gx,gy,gz] and magnetometer data [mx,my,mz]
Hmm, my comment have gone somewhere...
It's not right to set bw = bw+x(1:3,1);
Check what happens to the covariance matrix if it's still positive-definite when the filter diverges;
Try to add this P = (P+P')/2 to the code;
Tune system noise and measurement noise of the filter;
Generate hour-long test data with [dWb, q, dt, q_init, fb, mb, fn, mn, bw] = reference_data_spin_cone and check if it happens there;
I have been making some tests and I believe that there is a mistake on the gyro bias update inside the ahrs_quat.m.
If you try to estimate the orientation of a non-moving object for more than (approximately) 45 minutes, the estimation (quaternions) will diverge, so the gyro bias estimation.
If one removes the gyro bias update, such error is also removed. The problem is that bias update is a very important tool, therefore, I have tried some solutions and one that appears to work is to change the original bw = bw+x(4:6,1); to bw = bw+x(1:3,1);
But I don't know if it is correct.
If you guys have any information about it, I would be very happy to listen.
Strictly speaking accelerometer doesn't measure the gravity - it can only measure active forces applied to the object. But as long as we support this object from falling down - here's our active force. So - if our object is standing still or moving without the acceleration with constant velocity then by observing this active force that keeps us from falling we can estimate the direction where the gravity vector points to - it's all we need for the AHRS
To estimate the attitude of some object relative to some navigation frame we need to measure the projections of two non-collinear vectors on the axes of object coordinate frame and we need to know the corresponding projections of these same vectors on the axis of navigation frame. Here on Earth we've got two nice vectors - gravity vector and magnetic field vector. There are maps for both these vectors, particularly in AeroSpace Blockset for Simulink.
If you want to keep things simple then just place your device on horizontal surface, point its X axis toward the North, average the readings of the accelerometer and magnetometers for some time - these values will become your navigation-frame projections. Gravity should be close to [0 0 -1], of course, but magnetic field vector will look nothing like [1 0 0] - it'll depend on where you are on Earth :) I use [1 0 0 ] just as an example!
HI Vlad, can I ask a question about the KF here? So the measurement true value is gravity and mag vector in like earth frame which we already know as [0 0 -1] and [ 1 0 0], the measured is these two vectors in earth frame transferred using body to earth frame quaternion and accelerometer measurements. My question is how can we know which vector is gravity vector in the body frame just using accelerometer? Thank you so much !
Sure - gyros are meant to measure the dynamic motion. Gyros in rad/sec, and units for acc and magn are not that important as these vectors are normalized inside the algorithm - we basically need only directions that these vectors point to in order to correct the attitude.
Hi Vlad, is your code works even if the gyros are turned on during dynamic motion?
What is the units of the gyros, acc and the magnometers?
What I mean with the orientation of your sensors is the position of x, y and z measurements in your vector.
Here there is a link for the Figure 2 generate by Gyrolib: https://ibb.co/dksHdT
Description: The sensor has a relatively stable roll axis while it turn in the pitch and yaw axes.
I meant the Euler angles. Currently, I am working on adapting and understanding your code. One thing that I have noticed is that the orientation of your sensors might be different from mine. Could you tell me what are the orientation of your sensors (gyro, acc and mag)?
What do you mean by negative rotation - the way it looks in the animation, or Euler angles signs?
Hi Vald, it is me again.
I have been using and making some benchmarks over your solution and I have found something that was not expected by me nor present in others Fusion Methods, such as Mahony et al, Madgwick et al, BNO055 proprietary fusion algorithm...
The thing is:
Rotating CW the board along the Z axis (Yaw) gives me a positive rotation (positive derivative), however if a change the pitch up to a limit (let's say 90°), rotating CW the board along the Z axis (Yaw) gives me a NEGATIVE rotation.
I cannot add a picture here, but if you are interest I can explain this in more detail through email or something similar.
Hey, Felipe! Nope, sorry I've nether written one on the AHRS, so feel free to use it as it is. Or you can cite this place https://sites.google.com/site/gyrolib/home if you really need it.
Hey again, is there any academic paper that I could cite while using this code?
Hi Vlad, thank you for your fast and correct answer.
Analysing your new function I realized that my mistake was not normalizing the accelerometer and magnetometer data.
I added test_ahrs_quat_data function that allows to test ahrs_quat algorithm with real sensors data, you can run it like this: [Euler_, bw_, Angles_] = test_ahrs_quat_data(dWb, Fb, Mb, fn, mn, dt);
Sorry, I should have been more specific: I am loading the fmt_1010.mat and running:
P(1:3,1:3)=diag([1e-2, 1e-2, 1e-2]); %attitude errors in radian
P(4:6,4:6)=diag([1e-6, 1e-6, 1e-6]); %gyro bias errors in rad/s
q = [ones(length(Mb),1) zeros(length(Mb),3)];
bw = [0.01; -0.01; 0.01];
for i =1:length(Mb)-1
[q(i+1,:), P2, bw] = ahrs_quat(q(i,:), P, bw, dWb(i,:)', Fb(i,:)', Mb(i,:)', fn, mn, dt);
Using the provided data with the ahrs_quat.m or any other function that calls it gives me this message:
Warning: Matrix is singular, close to singular or badly scaled. Results may be inaccurate. RCOND = NaN.
> In ahrs_quat (line 93)
In ahrs_quat (line 93)
The P matrix is going to inf and doing so it is not invertible anymore. Could you take a look at that problem?
Find the treasures in MATLAB Central and discover how the community can help you!Start Hunting!