dRonin  adbada4
dRonin firmware
 All Data Structures Namespaces Files Functions Variables Typedefs Enumerations Enumerator Macros Groups Pages
insgps13state.c File Reference
#include "insgps.h"
#include "physical_constants.h"
#include <math.h>
#include <stdint.h>

Go to the source code of this file.

Macros

#define NUMX   13
 
#define NUMW   9
 
#define NUMV   10
 
#define NUMU   6
 

Functions

static void CovariancePrediction (float F[NUMX][NUMX], float G[NUMX][NUMW], float Q[NUMW], float dT, float P[NUMX][NUMX])
 
static 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)
 
static void RungeKutta (float X[NUMX], float U[NUMU], float dT)
 
static void StateEq (float X[NUMX], float U[NUMU], float Xdot[NUMX])
 
static void LinearizeFG (float X[NUMX], float U[NUMU], float F[NUMX][NUMX], float G[NUMX][NUMW])
 
static void MeasurementEq (float X[NUMX], float Be[3], float Y[NUMV])
 
static void LinearizeH (float X[NUMX], float Be[3], float H[NUMV][NUMX])
 
uint16_t ins_get_num_states ()
 
void INSGPSInit ()
 Reset the internal state variables and variances. More...
 
void INSSetArmed (bool armed)
 Set the current flight state. More...
 
void INSGetState (float *pos, float *vel, float *attitude, float *gyro_bias, float *accel_bias)
 Get the current state estimate. More...
 
void INSGetVariance (float *p)
 
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 gyro_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 (float baro_var)
 
void INSSetMagNorth (const float B[3])
 
void INSStatePrediction (const float gyro_data[3], const float accel_data[3], float dT)
 Compute an update of the state estimate. More...
 
void INSCovariancePrediction (float dT)
 Compute an update of the state covariance. More...
 
void INSCorrection (const float mag_data[3], const float Pos[3], const float Vel[3], float BaroAlt, uint16_t SensorsUsed)
 Correct the state and covariance estimate based on the sensors that were updated. More...
 

Variables

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