INS_EIF

Extended Information Filter for Inertial Navigation: UAV Attitude determination with multisensor/multirate systems

Synopsis

Matlab development and Autogenerated code for an Inertial navigation system using Extended Information Filter for multi IMU/GPS and multi sampling rates architecture.

Code implemented and run in SMT32F103:

Requirements

(Optional) Requires Matlab 2015a or above

Installation

Usage

See examples/IKARUS autopilot/main.c for reference.

Declarations and initialization

Add libraries

#include "EIF_n_dof.h"
#include "EIF_init.h"

Initialize filter variables:

 //EIF  variables
float ypr_eif[2];	
real_T eul[3] ={0,0,0};
int32_T number_sensors = 2; // Number of sensor being used
char_T sensors[8] ={ 'g', 'y', 'r', 'o' , 'a', 'c', 'c', 'e' };// Add here which ever number and type of sensors

real_T X[7],X_out[7];
real_T P[49],P_out[49];
real_T Q[36];
real_T sigmas[2];

real_T AT;// loop delta time
real_T measures[6];

Initialize EIF:

EIF_init( eul, X, P, Q,sigmas);

Notes: See EIF_init/EIF_init.c for initialization tuning

Main loop at AT rate:

Compute AT in seconds: Elapsed delta time since last execution. See RTOS funtionalities for loop rate time
Execute filter:

EIF_n_dof( (const real_T*) X,(const real_T*) P,(const real_T*)Q,(const real_T*) sigmas,(const real_T*) measures , (const real_T) AT,  eul,X_out, P_out);	   // AT has to be defined correctly!!!!

// Update X and P
memcpy(X,X_out,7*sizeof(real_T));
memcpy(P,P_out,49*sizeof(real_T));	
 
ypr_eif[0] = eul[1]*(180.0/PI);	 // Pitch in degs
ypr_eif[1] = eul[0]*(180.0/PI);	 // Roll in degs
eul[2]*(180.0/PI);                // Yaw/heading in degs.

//EIF angles
pitch = ypr_eif[0];
roll = ypr_eif[1];

Tests and Demo Video

Contributors

@David Torres Ocaña david.torres.ocana@gmail.com

THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL KARL KURZER BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.

DavidTorresOcana/INS_EIF