Using the library uNavAHRS

[brightproject]

In my project to create a spatial position device, until recently I used a simple Madwick filter. Everything works fine on the ground. After testing in flight, it is clear that the filter cannot cope with the compensation of centrifugal forces. As far as I understand, the uNavAHRS project is @flybrianfly old development? I managed to configure uNavAHRS to work with the development board. The information in the GitHub repository is scanty, the issue cannot be opened, and there are also few examples of work. I'm wondering if the uNavAHRS code can compensate for centrifugal forces if I use an accelerometer, gyroscope and magnetometer?

Orientation: 83.48 -13.21 -17.41
                Accel X: -3.68  Y: -7.82        Z: 2.30 m/s^2
                Gyro X: 0.02 Y: -0.07 Z: -0.06 rad/s
                Mag X: -25.45   Y: 24.55        Z: -34.90  uT
-0.22   1.48    -0.29   2.79    0.100868        -0.767209       1.166372        7305
Orientation: 84.91 -12.64 -16.72
                Accel X: -3.68  Y: -7.82        Z: 2.30 m/s^2
                Gyro X: 0.05 Y: -0.05 Z: -0.06 rad/s
                Mag X: -26.18   Y: 24.73        Z: -41.02  uT
-0.15   1.48    -0.35   2.73    0.100868        -0.767209       1.166372        6879
Orientation: 84.99 -8.71 -20.04
                Accel X: -3.68  Y: -7.82        Z: 1.84 m/s^2
                Gyro X: 0.03 Y: -0.07 Z: -0.06 rad/s
                Mag X: -26.73   Y: 24.64        Z: -30.92  uT
-0.20   1.53    -0.14   2.94    -0.088799       -0.892257       0.971080        6679
Orientation: 87.46 -11.52 -7.97
                Accel X: -3.68  Y: -8.28        Z: 2.30 m/s^2
                Gyro X: 0.03 Y: -0.06 Z: -0.04 rad/s
                Mag X: -27.18   Y: 24.73        Z: -30.92  uT
-0.25   1.61    -0.01   3.07    -0.230193       -0.947510       0.882849        7307
Orientation: 92.34 -14.28 -0.55
                Accel X: -3.22  Y: -8.28        Z: 2.30 m/s^2
                Gyro X: 0.05 Y: -0.05 Z: -0.04 rad/s
                Mag X: -27.45   Y: 24.82        Z: -32.24  uT
-0.28   1.70    0.06    3.14    -0.307554       -0.951104       0.859559        7344
Orientation: 97.51 -16.12 3.22
                Accel X: -3.68  Y: -7.36        Z: 2.76 m/s^2
                Gyro X: 0.03 Y: -0.06 Z: -0.04 rad/s
                Mag X: -27.91   Y: 24.55        Z: -33.98  uT
-0.34   1.81    0.17    3.25    -0.412972       -0.955774       0.810136        7284
Orientation: 103.57 -19.65 9.80

if (Filter.update(gx, -gy, gz, ax, ay, -az, mx, my, mz)) {
SerialPort.print(Filter.getPitch_rad());
SerialPort.print("\t");
SerialPort.print(Filter.getRoll_rad());
SerialPort.print("\t");
SerialPort.print(Filter.getYaw_rad());
SerialPort.print("\t");
SerialPort.print(Filter.getHeading_rad());
SerialPort.print("\t");
SerialPort.print(Filter.getGyroBiasX_rads(),6);
SerialPort.print("\t");
SerialPort.print(Filter.getGyroBiasY_rads(),6);
SerialPort.print("\t");
SerialPort.print(Filter.getGyroBiasZ_rads(),6);
SerialPort.print("\t");
SerialPort.println(tstop - tstart);

Also I didn't quite understand the function Filter.getGyroBiasX_rads(); How does it measure errors and compensate for them itself? I substitute the following values into the filter:

1. accelerometers in m/sec^2
2. gyroscopes in the form of rad/sec
3. in the form of uTesla

But nowhere is it indicated in what units the magnetic field data should be supplied to the filter. As far as I understand, you are using the library units, she defines common unit conversions. When running the uNavAHRS code, I often get nan.

                Accel X: 0.92   Y: -9.20        Z: 0.46 m/s^2
                Gyro X: -0.28 Y: 0.08 Z: -0.02 rad/s
                Mag X: -45.45   Y: 41.27        Z: -21.22  uT
0.14    1.54    0.68    4.53    0.186303        -0.057557       0.134090        7324
Orientation: 87.97 7.84 39.14
                Accel X: 0.92   Y: -9.20        Z: 0.00 m/s^2
                Gyro X: -0.17 Y: 0.09 Z: -0.04 rad/s
                Mag X: -45.55   Y: 42.18        Z: -19.39  uT
nan     nan     nan     nan     nan     nan     nan     7196
Orientation: nan nan nan
                Accel X: 0.46   Y: -9.20        Z: -0.46 m/s^2
                Gyro X: -0.07 Y: 0.04 Z: -0.03 rad/s
                Mag X: -45.64   Y: 42.45        Z: -19.80  uT
nan     nan     nan     nan     nan     nan     nan     7135
Orientation: nan nan nan

In a forum thread several years ago, there were lively discussions, but now they have stopped... and I realized that the project has ceased to be relevant. I am also puzzled by obtaining data for the operation of a slip indicator - a sealed tube with a ball inside. It would be cool if the library provided values to determine coordinated reversal. My programming skills are not good enough and I read more theory. I found the code for the iNav project, where, as I understand it, they were able to compensate for centrifugal forces in flight and, depending on the inclination, reduce the effect (weight of the accelerometer) of the centrifugal acceleration force. As far as I understand, the force of centrifugal acceleration can be compensated by knowing the turning speed, and somewhere I found a solution using airspeed and GPS. And @bolderflight probably used this method in the uNavINS project. But what if the sensor is only an accelerometer and a gyroscope, and possibly a magnetometer? I hope you can shed some light on my research. Can I use this code for the purpose of determining orientation in space? Really appreciate the help.

[flybrianfly]

The uNavAHRS tried a few different KF and EKF filters using IMU and magnetometer data to determine attitude. Integrating the gyro is used for high rate updates of the orientation (time updates) and accel / mag is used to correct for drift in the integration (measurement updates) and estimate gyro errors (i.e. the gyro bias). There are ways to avoid performing the measurement update during acceleration or a magnetic disturbance; however, in a long coordinated turn, you'll always have issues since the acceleration is low the accelerometer during measurement updates will drive the attitude back to wings level. There are tricks that can be done to help, by feeding in airspeed, but they aren't great.

The 15 state EKF in this library is just an updated version of uNavIns. Gyro and accel are integrated in the time update to estimate position and attitude. GNSS data is used as a measurement update to arrest the drift in the integration and to estimate gyro and accel biases. Magnetometer is no longer needed since we're using GNSS data as a truth source.

Highly recommend the Optimal State Estimation book by Dan Simon. It's a complex topic that requires tuning for the application. We use the 15 state EKF often for our projects, but it's hard to make it into a library that will universally work with Arduino across all the microcontrollers, sensors, and applications people will try it on.

[brightproject]

Thank you, I read theoretical materials on Kalman filtration from time to time, but it’s been this way since childhood that the “dry” theory never sticks in my head. I have become familiar with this document and with this forum thread. In a turn, we have two vectors - and the resulting one becomes perpendicular to the wing. I used to think that a quaternion allows us to avoid gimbal locks, and Kalman or Majwick filters help fight interference and compensate for centrifugal forces, but it turns out that everything is much more complicated. But as I wrote above, it’s difficult for me to dive into theory without practice ... when I don’t see a working result, the theory immediately becomes doubly unclear. When you see manifestations of some process or mechanism in action, then, taking into account the theory, you understand its work. Therefore, my path to the theory of filtration lies through practices, incl. using projects like yours. Special thanks for this. I didn’t see answers to my questions in your answer - it turns out that iNavAHRS will not solve the issue with centrifugal forces? And do you also need to input data from GNSS to the accelerometer, gyroscope and magnetometer? You write

however, in a long coordinated turn, you'll always have issues since the acceleration is low the accelerometer during measurement updates will drive the attitude back to wings level.

The simplest calculation of roll and pitch angles can be performed using acceleration data, as an example: float roll = atan2(accelEvent.acceleration.y, accelEvent.acceleration.z); float pitch = atan2(accelEvent.acceleration.x, sqrt(accelEvent.acceleration.y * accelEvent.acceleration.y + accelEvent.acceleration.z * accelEvent.acceleration.z)); The simplest calculation of roll and pitch angles can be performed using acceleration data, as an example: So maybe we should do what we did before, in the era of mechanical rotating gyroscopes - at the moment of a turn, incl. roll simply turned off the gyroscope correction. Somehow you can add a function in the uNavAHRS filtering to reduce the significance or weight of data from the accelerometer, and increase the weight of the gyroscope. And upon completion of the turn (roll), the data from the accelerometer again has the same weight. This section of code already works in the case of changing acceleration values.

https://github.com/FlyTheThings/uNavAHRS/blob/5437732ccfb100f5b8f93ce26a88fcd6a0be928c/uNavAHRS.cpp#L98

      if (accelUpdated_) {
        _accelCount++;
        _accelDelta(0,0) = ax - _accelMean(0,0);
        _accelMean(0,0) = _accelMean(0,0) + _accelDelta(0,0) / ((float)_accelCount);
        _accelDelta2(0,0) = ax - _accelMean(0,0);
        _accelM2(0,0) = _accelM2(0,0) + _accelDelta(0,0) * _accelDelta2(0,0);
        _accelDelta(1,0) = ay - _accelMean(1,0);
        _accelMean(1,0) = _accelMean(1,0) + _accelDelta(1,0) / ((float)_accelCount);
        _accelDelta2(1,0) = ay - _accelMean(1,0);
        _accelM2(1,0) = _accelM2(1,0) + _accelDelta(1,0) * _accelDelta2(1,0);
        _accelDelta(2,0) = az - _accelMean(2,0);
        _accelMean(2,0) = _accelMean(2,0) + _accelDelta(2,0) / ((float)_accelCount);
        _accelDelta2(2,0) = az - _accelMean(2,0);
        _accelM2(2,0) = _accelM2(2,0) + _accelDelta(2,0) * _accelDelta2(2,0);
        if (_accelCount > 2) {
          _accelVariance(0,0) = _accelM2(0,0)/((float)(_accelCount-1));
          _accelVariance(1,0) = _accelM2(1,0)/((float)(_accelCount-1));
          _accelVariance(2,0) = _accelM2(2,0)/((float)(_accelCount-1));
        }
      }

You can also turn it on/off by angular velocity, if it exceeds some limits, and also by roll angle greater than a certain value, for example, when the roll angle is more than 7-10 degrees, turn off the data from the accelerometer and trust only the gyroscope and magnetometer (if available) . The roll angle at which this correction of centrifugal forces will “trigger” can also be adjusted in the filter. Currently I don't see any filter settings at all other than setting the filter initialization time, which is 2 minutes by default. Filter.setInitializationDuration(600000); I also found a code that compensates for centrifugal force, but it is built on vectors, while NavAHRS uses Eigen matrix. One of these days I plan to fly around the device with a filter Madgwick, with the addition of a calibrated magnetometer and the uNavAHRS filter, also with a calibrated accelerometer, gyroscope and magnetometer. But I have concerns that despite all my efforts, in turns the bank angle will change following the wing and I will not achieve the desired results. Can I ask you about the Gyro Bias? Filter.getGyroBiasX_rads(); Do they simply carry an exponential function, or should these values be subtracted from the initial values of the angular velocity? As far as I understand, the uNavAHRS library code corrects angular velocity offsets automatically.

[flybrianfly]

Quaternions do avoid gimbal lock, but that's only a concern at +/- 90 degrees of pitch using typical Euler rotations.

The issue with turning off measurement updates for a long period of time is that the gyro integration will drift. You can compensate if you have the body velocity, which is what the code you cite is doing and what VectorNav does with the VN-100, but that requires an additional source of data.

There are tons of commercial products similar to what you show - they are typically employing schemes of pre-filtering the data (low pass filter the accels, high pass filter the gyro) and selectively turning off the measurement update during acceleration or magnetic disturbances. They all would still have problems in a prolonged turn. Out of the box, uNavAHRS isn't employing any of those tricks.

The filter initialization time was a way of trying to auto tune the EKF based on the sensor noise. It didn't work very well and is one of the reasons why you won't see that code on my repos. Our 15 state EKF has hand-tuned gains.

If I remember correctly, the biases were removed in the code and that output was just there to see how the filter was converging.

[brightproject]

My regards When I use the uNavAHRS filter, when I tilt the board with sensors to the left, I get a negative roll, and when I tilt it to the right, I get a positive roll. In this case, the pitch shows +/-10. This is all good, but when I change the pitch of the board, the roll also changes significantly. All signs and axes of the accelerator, gyroscope and magnetometer are combined. The direction of acceleration is also taken into account - I don’t understand what’s wrong. Well, the yaw does not change when rotating around the Z axis. And when, after pitching, you return the board to the horizon, the visual model, according to the roll, pitch and yaw data, begins to depict a Dutch step. When you used uNavAHRS, did you notice any similar actions? Tell me, you said on the pjrc forum that you wanted to leave the possibility for UsIns to work in the absence of a GPS signal. In my device, I don’t want to complicate the circuitry, add a GNSS receiver, additional power and, of course, an external antenna. Have you tried using uNavINS only with data from the accelerometer, gyro and magnetometer, or is this filter fundamentally ineffective without GPS? In some threads on Github, there were lively discussions, including iNav, about the use of some kind of direction cosine matrix DCM. I apologize for mentioning another one in your product thread, but these are the only information resources that discuss the practical application of the implementation to eliminate centrifugal forces. Although I am indifferent to copters, they still have very powerful practice in terms of flight, aerodynamics and working with MEMS sensors. So, in one of the threads, the guys discuss working without using GPS, and substitute the reference speed value into the filter. I thought, what if I did the same in your uNavIns filter? Only two speed components are substituted into the filter: double vn: GPS velocity in the North direction, units are m/s. double ve: GPS velocity in the East direction, units are m/s. or even three, also the vertical component of the speed, and all this is taken from GNSS data. Probably we can take the vertical component of the speed from the barometric sensor, and substitute some kind of reference for the northern and eastern components. At the same time, one must understand that then the flight must be controlled. Those. take off without banking, establish a straight flight at the speed that we have set as the “reference” and only after that perform roll maneuvers, but do not overdo it, do not exceed 30-35 degrees and do not make long turns. Again, all these settings are selected empirically. In general, I was interested in the opportunity to perform tests and flights in the simulator. I'm thinking X-plane or RealFlight Evolution. All that remains is to figure out how to insert your AHRS code into it. Otherwise, it’s very expensive and time-consuming when you write code, test it on the table and then fly on an airplane. At the same time, we are guessing about coffee. P.S. Could you add an experimental function to the uNavINS library, which would make it possible to adjust the weight of the accelerometer, depending on the angular velocity, as well as disable GPS?