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]