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.
-
Go to the function HandleUserOutputPacket in UserMessaging.c
-
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]; -
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]; -
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
