dRonin  adbada4
dRonin firmware
 All Data Structures Namespaces Files Functions Variables Typedefs Enumerations Enumerator Macros Groups Pages
Macros | Functions | Variables
INSGPS
Utilities

INSGPS is a joint attitude and position estimation EKF. More...

Macros

#define NUMX   14
 
#define NUMW   10
 
#define NUMV   10
 
#define NUMU   6
 
#define NUMX   16
 
#define NUMW   12
 
#define NUMV   10
 
#define NUMU   6
 

Functions

void CovariancePrediction (float F[NUMX][NUMX], float G[NUMX][NUMW], float Q[NUMW], float dT, float P[NUMX][NUMX])
 
void SerialUpdate (float H[NUMV][NUMX], float R[NUMV], float Z[NUMV], float Y[NUMV], float P[NUMX][NUMX], float X[NUMX], uint16_t SensorsUsed)
 
void RungeKutta (float X[NUMX], float U[NUMU], float dT)
 
void StateEq (float X[NUMX], float U[NUMU], float Xdot[NUMX])
 
void LinearizeFG (float X[NUMX], float U[NUMU], float F[NUMX][NUMX], float G[NUMX][NUMW])
 
void MeasurementEq (float X[NUMX], float Be[3], float Y[NUMV])
 
void LinearizeH (float X[NUMX], float Be[3], float H[NUMV][NUMX])
 
uint16_t ins_get_num_states ()
 
void INSGPSInit ()
 
void INSSetArmed (bool armed)
 Set the current flight state. More...
 
void INSGetState (float *pos, float *vel, float *attitude, float *gyro_bias, float *accel_bias)
 
void INSGetVariance (float *var_out)
 
void INSResetP (const float *PDiag)
 
void INSSetState (const float pos[3], const float vel[3], const float q[4], const float gyro_bias[3], const float accel_bias[3])
 
void INSPosVelReset (const float pos[3], const float vel[3])
 
void INSSetPosVelVar (float PosVar, float VelVar, float VertPosVar)
 
void INSSetGyroBias (const float gyro_bias[3])
 
void INSSetAccelBias (const float accel_bias[3])
 
void INSSetAccelVar (const float accel_var[3])
 
void INSSetGyroVar (const float gyro_var[3])
 
void INSSetMagVar (const float scaled_mag_var[3])
 
void INSSetBaroVar (const float baro_var)
 
void INSSetMagNorth (const float B[3])
 
void INSLimitBias ()
 
void INSStatePrediction (const float gyro_data[3], const float accel_data[3], float dT)
 
void INSCovariancePrediction (float dT)
 
void INSCorrection (const float mag_data[3], const float Pos[3], const float Vel[3], float BaroAlt, uint16_t SensorsUsed)
 

Variables

float F [NUMX][NUMX]
 
float G [NUMX][NUMW]
 
float H [NUMV][NUMX]
 
float Be [3]
 
float P [NUMX][NUMX]
 
float X [NUMX]
 
float Q [NUMW]
 
float R [NUMV]
 
float K [NUMX][NUMV]
 
float F [NUMX][NUMX]
 
float G [NUMX][NUMW]
 
float H [NUMV][NUMX]
 
float Be [3]
 
float P [NUMX][NUMX]
 
float X [NUMX]
 
float Q [NUMW]
 
float R [NUMV]
 
float K [NUMX][NUMV]
 

Detailed Description

INSGPS is a joint attitude and position estimation EKF.

Macro Definition Documentation

#define NUMU   6

Definition at line 41 of file insgps14state.c.

#define NUMU   6

Definition at line 41 of file insgps16state.c.

#define NUMV   10

Definition at line 40 of file insgps14state.c.

#define NUMV   10

Definition at line 40 of file insgps16state.c.

#define NUMW   10

Definition at line 39 of file insgps14state.c.

#define NUMW   12

Definition at line 39 of file insgps16state.c.

#define NUMX   14

Definition at line 38 of file insgps14state.c.

#define NUMX   16

Definition at line 38 of file insgps16state.c.

Function Documentation

void CovariancePrediction ( float  F[NUMX][NUMX],
float  G[NUMX][NUMW],
float  Q[NUMW],
float  dT,
float  P[NUMX][NUMX] 
)

Definition at line 453 of file insgps14state.c.

uint16_t ins_get_num_states ( )

Definition at line 73 of file insgps14state.c.

void INSCorrection ( const float  mag_data[3],
const float  Pos[3],
const float  Vel[3],
float  BaroAlt,
uint16_t  SensorsUsed 
)

Definition at line 354 of file insgps14state.c.

void INSCovariancePrediction ( float  dT)

Definition at line 349 of file insgps14state.c.

void INSGetState ( float *  pos,
float *  vel,
float *  attitude,
float *  gyro_bias,
float *  accel_bias 
)

Get the current state estimate (null input skips that get)

Parameters
[out]posThe position in NED space (m)
[out]velThe velocity in NED (m/s)
[out]attitudeQuaternion representation of attitude
[out]gyros_biasEstimate of gyro bias (rad/s)
[out]accel_biasEstiamte of the accel bias (m/s^2)

Definition at line 152 of file insgps14state.c.

void INSGetVariance ( float *  var_out)

Get the variance, for visualizing the filter performance

Parameters
outvar_out The variances

Definition at line 190 of file insgps14state.c.

void INSGPSInit ( )

Definition at line 78 of file insgps14state.c.

void INSLimitBias ( )

Definition at line 303 of file insgps14state.c.

void INSPosVelReset ( const float  pos[3],
const float  vel[3] 
)

Definition at line 228 of file insgps14state.c.

void INSResetP ( const float *  PDiag)

Definition at line 196 of file insgps14state.c.

void INSSetAccelBias ( const float  accel_bias[3])

Definition at line 265 of file insgps14state.c.

void INSSetAccelVar ( const float  accel_var[3])

Definition at line 270 of file insgps14state.c.

void INSSetArmed ( bool  armed)

Set the current flight state.

Definition at line 131 of file insgps14state.c.

void INSSetBaroVar ( const float  baro_var)

Definition at line 291 of file insgps14state.c.

void INSSetGyroBias ( const float  gyro_bias[3])

Definition at line 258 of file insgps14state.c.

void INSSetGyroVar ( const float  gyro_var[3])

Definition at line 277 of file insgps14state.c.

void INSSetMagNorth ( const float  B[3])

Definition at line 296 of file insgps14state.c.

void INSSetMagVar ( const float  scaled_mag_var[3])

Definition at line 284 of file insgps14state.c.

void INSSetPosVelVar ( float  PosVar,
float  VelVar,
float  VertPosVar 
)

Definition at line 248 of file insgps14state.c.

void INSSetState ( const float  pos[3],
const float  vel[3],
const float  q[4],
const float  gyro_bias[3],
const float  accel_bias[3] 
)

Definition at line 210 of file insgps14state.c.

void INSStatePrediction ( const float  gyro_data[3],
const float  accel_data[3],
float  dT 
)

Definition at line 324 of file insgps14state.c.

void LinearizeFG ( float  X[NUMX],
float  U[NUMU],
float  F[NUMX][NUMX],
float  G[NUMX][NUMW] 
)

Linearize the state equations around the current state estimate.

Parameters
[in]Xthe current state estimate
[in]Uthe control inputs
[out]Fthe linearized natural dynamics
[out]Gthe linearized influence of disturbance model

so the prediction of the next state is Xdot = F * X + G * U where X is the current state and U is the current input

For reference the state order (in F) is pos, vel, attitude, gyro bias, accel bias and the input order is gyro, bias

Definition at line 751 of file insgps14state.c.

void LinearizeH ( float  X[NUMX],
float  Be[3],
float  H[NUMV][NUMX] 
)

Linearize the measurement around the current state estiamte so the predicted measurements are Z = H * X

Definition at line 879 of file insgps14state.c.

void MeasurementEq ( float  X[NUMX],
float  Be[3],
float  Y[NUMV] 
)

Predicts the measurements from the current state. Note that this is very similar to LinearizeH except this directly computes the outputs instead of a matrix that you transform the state by

Definition at line 845 of file insgps14state.c.

void RungeKutta ( float  X[NUMX],
float  U[NUMU],
float  dT 
)

Definition at line 642 of file insgps14state.c.

void SerialUpdate ( float  H[NUMV][NUMX],
float  R[NUMV],
float  Z[NUMV],
float  Y[NUMV],
float  P[NUMX][NUMX],
float  X[NUMX],
uint16_t  SensorsUsed 
)

Definition at line 594 of file insgps14state.c.

void StateEq ( float  X[NUMX],
float  U[NUMU],
float  Xdot[NUMX] 
)

Definition at line 689 of file insgps14state.c.

Variable Documentation

float Be[3]

Definition at line 65 of file insgps14state.c.

float Be[3]

Definition at line 65 of file insgps16state.c.

float F[NUMX][NUMX]

Definition at line 63 of file insgps14state.c.

float F[NUMX][NUMX]

Definition at line 63 of file insgps16state.c.

float G[NUMX][NUMW]

Definition at line 63 of file insgps14state.c.

float G[NUMX][NUMW]

Definition at line 63 of file insgps16state.c.

float H[NUMV][NUMX]

Definition at line 63 of file insgps14state.c.

float H[NUMV][NUMX]

Definition at line 63 of file insgps16state.c.

float K[NUMX][NUMV]

Definition at line 68 of file insgps14state.c.

float K[NUMX][NUMV]

Definition at line 68 of file insgps16state.c.

float P[NUMX][NUMX]

Definition at line 66 of file insgps14state.c.

float P[NUMX][NUMX]

Definition at line 66 of file insgps16state.c.

float Q[NUMW]

Definition at line 67 of file insgps14state.c.

float Q[NUMW]

Definition at line 67 of file insgps16state.c.

float R[NUMV]

Definition at line 67 of file insgps16state.c.

float R[NUMV]

Definition at line 67 of file insgps14state.c.

float X[NUMX]

Definition at line 66 of file insgps16state.c.

float X[NUMX]

Definition at line 66 of file insgps14state.c.