Sylvain,

Just as Simon pointed out, I tried to look over your modifications to the EKF algorithm as well but the link doesn't exist.

Joe

Sylvain,

I will confer with the people who are currently developing the algorithms and who control the firmware to find out their thoughts on this. I will get back to you once I hear from them.

Joe

Sylvain:

What changes are you suggesting? Do you want to output the process covariance values to your system and want to add a getter to the firmware? Let me know and I will either answer you directly or forward you the most appropriate person.

Thanks,
Joe

Joris,

Can you send me your contact information? My email is jmotyka [at] aceinna.com

We have a fix for one magnetometer issue (scaling). If you are having this issue then the fix may help but it requires you to load new library files onto your unit. Please let me know if your magnetometer data changes but only slightly (zoom into the signal trace and move the unit around; it should change as you move the part).

If the signal does not change at all then there may be another issue that we need to address. If this is the case we will work with you to resolve this issue.

Joe

Hi Joris,

I am still looking into your issue but do not have a resolution for you yet. Can you tell me what the magnetic-field values are that your unit reports? If you zoom into the signal and move your part does the signal change at all (even in the 3rd or 4th decimal place) or is it unchanging? This information will go a long way in helping us troubleshoot your issue.

I will get back to you as soon as we have a resolution to your problem.

Joe

Joris:

Just to confirm, you are using the Compass project as it came from the OpenIMU platform, you haven't modified anything yet? Also, which output message are you using? c1?

Joe

Hi Ron,

The output you get over the debug line is debug code that is contained in the calibration code and, unfortunately, not made available to the user. When you issue the 'Get Stored Values' command (55 55 6D 61 01 07 81 C9), the values labeled 'Defaults' are the values currently in memory and stored in the EEPROM while the 'Estimates' are the ones resulting from the Magnetic-Alignment process. If you accept these estimates then they replace the values used to correct for hard/soft-iron effects and are saved to the EEPROM. At startup, they become the default values.

If you issue the 'Get Stored Values' command before you perform an alignment then the estimates are just the initial values for the magnetic-alignment variables. This is why you see the 0.0 and 1.0 values after you issue the command.

As to why you still get an ellipse when you plot the magnetic-field measurements after performing the magnetic-alignment, the hard/soft-iron estimates are only used to form the heading estimate, not correct the raw magnetic field measurements (as you state in your question). Without a full 3D estimate of the magnetic environment it is not possible to adjust the raw signal. The method by which the hard/soft-iron values are used to form the heading estimate is described at https://openimu.readthedocs.io/en/latest/algorithms/MeasurementModel.html#

The heading angle computed using the equation described in the measurement model is angle from magnetic-north. A value of 0.0 indicates that you are pointed north while a value of 90.0 indicates that you are pointed east.

Re: Example INS App

Adam:

While the baseline Kalman filter algorithm requires GPS measurements in order to provide position or velocity estimates, a few modifications will enable the algorithm to provide the information you would like. Please be aware, without GPS measurements to estimate the accelerometer bias and correct the EKF estimates, errors in the position and velocity will quickly increase (velocity grows linearly with time whereas position grows with time-squared).

e2 Output Message:
First, to determine where the e2 data is computed, we follow the chain of events backward:

• the e2 message is populated with data in the EKF output data structure, gEkFOutputData (UserMessaging.c)
• gEkFOutputData is loaded with the EKF estimates (gKalmanFilter) by the setter EKF_SetOutputStruct(), called in RunUserNavAlgorithm()
• Finally, the estimates in gKalmanFilter are populated in the Kalman filter prediction and update functions (PredictFunctions.c and UpdateFunctions.c). This is where we start modifying the firmware.

Why position is not provided in baseline algorithm:
As you are not providing GPS measurements to the system, the Kalman filter will not transition from the Low_Gain_AHRS algorithm to the INS algorithm (as GPS data is needed for this transition to occur - see the function LG_To_INS_Transition_Test, found in SelectState.c, for information on the transition). At present, unless the algorithm is operating in INS mode, the Position and Velocity estimates are set to zero in the function _PredictStateEstimate(), found in PredictFunctions.c.

Code Modifications to get Position and Velocity:
The simplest modification to the firmware to obtain position and velocity estimates is to change the if-statement (on line 188 of PredictFunctions.c) from:

if( gAlgorithm.state > LOW_GAIN_AHRS ) {

to

if( gAlgorithm.state >= HIGH_GAIN_AHRS ) {

This will cause the algorithm to integrate the accelerometer signal (to compute position and velocity) as soon as the system begins calling the algorithm.

Description of EKF operational modes: the EKF begins operating in HIGH_GAIN where the Kalman gain is set high so the attitude solution converges quickly. It then transitions to LOW_GAIN_MODE where nominal (operational) values are used. Finally, if the system detects GPS messaging, the EKF transitions to INS mode where position and velocity (in addition to attitude) are estimated.

One final thing that I would do… Before the corrected-acceleration values (correctedAccel_B) are computed in _PredictStateEstimate(), set gKalmanFilter.accelBias_B to zero (for all axes), as follows:

// aCorr_B = aMeas_B - aBias_B
gKalmanFilter.accelBias_B[X_AXIS] = 0.0;
gKalmanFilter.accelBias_B[Y_AXIS] = 0.0;
gKalmanFilter.accelBias_B[Z_AXIS] = 0.0;

// gEKFInputData.accel_B in g's, convert to m/s^2 for integration
gKalmanFilter.correctedAccel_B[X_AXIS] = (real)(gEKFInputData.accel_B[X_AXIS] * GRAVITY) -
                                         gKalmanFilter.accelBias_B[X_AXIS];
gKalmanFilter.correctedAccel_B[Y_AXIS] = (real)(gEKFInputData.accel_B[Y_AXIS] * GRAVITY) -
                                         gKalmanFilter.accelBias_B[Y_AXIS];
gKalmanFilter.correctedAccel_B[Z_AXIS] = (real)(gEKFInputData.accel_B[Z_AXIS] * GRAVITY) -
                                         gKalmanFilter.accelBias_B[Z_AXIS];

This will prevent rogue bias-values from affecting the integration (small bias values are quickly integrated into large errors).

If, in the future you wish to use GPS, you can keep the logic the same, just remove the part that sets the bias to zero. In this case there will be a rapid change and convergence in the position and velocity data once the transition to the INS mode occurs.

Modification of e2 Output Message:
One final step is needed to output NED position in the e2 message. Currently this message output latitude, longitude, and altitude, not NED position. Ideally you should create another output message, e3, that replaces latitude, etc. with position. However, you can modify the e2 message to quickly get the information you are interested in.

1. Go to the function HandleUserOutputPacket in UserMessaging.c

2. Find the switch statement that populates the e2 message, “case USR_OUT_EKF2:”, and the section that populates the Lat/Lon/Alt data (starting at line 817)

            // Set the pointer of the algoData array to the payload
            double *algoData_4 = (double*)(algoData_3);
            double lla[NUM_AXIS];
            EKF_GetEstimatedLLA(lla);
            *algoData_4++ = (double)lla[LAT];
            *algoData_4++ = (double)lla[LON];
            *algoData_4++ = (double)lla[ALT];
   

3. Modify it as follows…

            // Set the pointer of the algoData array to the payload
            double *algoData_4 = (double*)(algoData_3);
            float pos[NUM_AXIS];
            EKF_GetEstimatedPosition(pos);
            *algoData_4++ = (double)pos[X_AXIS];
            *algoData_4++ = (double)pos[Y_AXIS];
            *algoData_4++ = (double)pos[Z_AXIS];
   

4. Recompile and load code onto OpenIMU

In the output message, LLA will be replaced with position data. If you would like LLA, you need to convert position information to LLA. The functions to do this are found in UpdateFunctions.c (PosNED_To_PosECEF() and ECEF_To_LLA()). Currently this is called in the INS update section, which is not called unless the system is provided GPS messages.

Finally, the openimu.json file describing the e2 message should be changed to reflect that the LLA data is now position data. Data in the figures on the Aceinna Navigation Studio will be correct although the labels will not be.

Step-by-step instructions on creating your own applications and how to modify the openimu.json file is found at https://openimu.readthedocs.io/en/latest/apps.html

Joe