SoloGimbalEKF Class Reference

#include <SoloGimbalEKF.h>

Collaboration diagram for SoloGimbalEKF:

Classes
struct	state_elements

Public Types
typedef float	ftype
typedef ftype	Vector2[2]
typedef ftype	Vector3[3]
typedef ftype	Vector5[5]
typedef ftype	Vector6[6]
typedef ftype	Vector8[8]
typedef ftype	Vector9[9]
typedef ftype	Vector10[10]
typedef ftype	Vector11[11]
typedef ftype	Vector13[13]
typedef ftype	Vector14[14]
typedef ftype	Vector15[15]
typedef ftype	Vector22[22]
typedef ftype	Vector31[31]
typedef ftype	Vector34[34]
typedef ftype	Matrix3[3][3]
typedef ftype	Matrix22[22][22]
typedef ftype	Matrix34_50[34][50]
typedef uint32_t	Vector_u32_50[50]

Public Member Functions
	SoloGimbalEKF (const AP_AHRS_NavEKF &ahrs)
void	RunEKF (float delta_time, const Vector3f &delta_angles, const Vector3f &delta_velocity, const Vector3f &joint_angles)
void	reset ()
void	getDebug (float &tilt, Vector3f &velocity, Vector3f &euler, Vector3f &gyroBias) const
void	getGyroBias (Vector3f &gyroBias) const
void	setGyroBias (const Vector3f &gyroBias)
void	getQuat (Quaternion &quat) const
bool	getStatus (void) const

Static Public Attributes
static const struct AP_Param::GroupInfo	var_info []

Private Member Functions
void	predictStates ()
void	predictCovariance ()
void	fuseVelocity ()
void	readMagData ()
void	fuseCompass ()
void	alignHeading ()
float	calcMagHeadingInnov ()
void	fixCovariance ()

Private Attributes
const AP_AHRS_NavEKF &	_ahrs
Vector13	states
struct SoloGimbalEKF::state_elements &	state
struct {
Vector3f   delAng
Vector3f   delVel
float   gPhi
float   gPsi
float   gTheta
}	gSense
float	Cov [9][9]
Matrix3f	Tsn
float	TiltCorrection
bool	newDataMag
uint32_t	StartTime_ms
bool	FiltInit
bool	YawAligned
float	cosPhi
float	cosTheta
float	sinPhi
float	sinTheta
float	sinPsi
float	cosPsi
uint32_t	lastMagUpdate
Vector3f	magData
uint32_t	imuSampleTime_ms
float	dtIMU
float	innovationIncrement
float	lastInnovation

Detailed Description

Definition at line 33 of file SoloGimbalEKF.h.

Member Typedef Documentation

ftype

typedef float SoloGimbalEKF::ftype

Definition at line 36 of file SoloGimbalEKF.h.

Matrix22

typedef ftype SoloGimbalEKF::Matrix22[22][22]

Definition at line 72 of file SoloGimbalEKF.h.

Matrix3

typedef ftype SoloGimbalEKF::Matrix3[3][3]

Definition at line 71 of file SoloGimbalEKF.h.

Matrix34_50

typedef ftype SoloGimbalEKF::Matrix34_50[34][50]

Definition at line 73 of file SoloGimbalEKF.h.

Vector10

typedef ftype SoloGimbalEKF::Vector10[10]

Definition at line 63 of file SoloGimbalEKF.h.

Vector11

typedef ftype SoloGimbalEKF::Vector11[11]

Definition at line 64 of file SoloGimbalEKF.h.

Vector13

typedef ftype SoloGimbalEKF::Vector13[13]

Definition at line 65 of file SoloGimbalEKF.h.

Vector14

typedef ftype SoloGimbalEKF::Vector14[14]

Definition at line 66 of file SoloGimbalEKF.h.

Vector15

typedef ftype SoloGimbalEKF::Vector15[15]

Definition at line 67 of file SoloGimbalEKF.h.

Vector2

typedef ftype SoloGimbalEKF::Vector2[2]

Definition at line 57 of file SoloGimbalEKF.h.

Vector22

typedef ftype SoloGimbalEKF::Vector22[22]

Definition at line 68 of file SoloGimbalEKF.h.

Vector3

typedef ftype SoloGimbalEKF::Vector3[3]

Definition at line 58 of file SoloGimbalEKF.h.

Vector31

typedef ftype SoloGimbalEKF::Vector31[31]

Definition at line 69 of file SoloGimbalEKF.h.

Vector34

typedef ftype SoloGimbalEKF::Vector34[34]

Definition at line 70 of file SoloGimbalEKF.h.

Vector5

typedef ftype SoloGimbalEKF::Vector5[5]

Definition at line 59 of file SoloGimbalEKF.h.

Vector6

typedef ftype SoloGimbalEKF::Vector6[6]

Definition at line 60 of file SoloGimbalEKF.h.

Vector8

typedef ftype SoloGimbalEKF::Vector8[8]

Definition at line 61 of file SoloGimbalEKF.h.

Vector9

typedef ftype SoloGimbalEKF::Vector9[9]

Definition at line 62 of file SoloGimbalEKF.h.

Vector_u32_50

typedef uint32_t SoloGimbalEKF::Vector_u32_50[50]

Definition at line 74 of file SoloGimbalEKF.h.

Constructor & Destructor Documentation

SoloGimbalEKF()

SoloGimbalEKF::SoloGimbalEKF

(

const AP_AHRS_NavEKF &

ahrs

)

Definition at line 31 of file SoloGimbalEKF.cpp.

Here is the call graph for this function:

Member Function Documentation

alignHeading()

void SoloGimbalEKF::alignHeading ( )

private

Definition at line 832 of file SoloGimbalEKF.cpp.

Referenced by RunEKF().

Here is the call graph for this function:

Here is the caller graph for this function:

calcMagHeadingInnov()

float SoloGimbalEKF::calcMagHeadingInnov ( )

private

Definition at line 850 of file SoloGimbalEKF.cpp.

Referenced by alignHeading(), and fuseCompass().

Here is the call graph for this function:

Here is the caller graph for this function:

fixCovariance()

void SoloGimbalEKF::fixCovariance ( )

private

Definition at line 908 of file SoloGimbalEKF.cpp.

Referenced by fuseCompass(), and fuseVelocity().

Here is the caller graph for this function:

fuseCompass()

void SoloGimbalEKF::fuseCompass ( )

private

Definition at line 685 of file SoloGimbalEKF.cpp.

Referenced by RunEKF().

Here is the call graph for this function:

Here is the caller graph for this function:

fuseVelocity()

void SoloGimbalEKF::fuseVelocity ( )

private

Definition at line 596 of file SoloGimbalEKF.cpp.

Referenced by RunEKF().

Here is the call graph for this function:

Here is the caller graph for this function:

getDebug()

void SoloGimbalEKF::getDebug	(	float &	tilt,
		Vector3f &	velocity,
		Vector3f &	euler,
		Vector3f &	gyroBias
	)		const

Definition at line 927 of file SoloGimbalEKF.cpp.

Here is the call graph for this function:

getGyroBias()

void SoloGimbalEKF::getGyroBias

(

Vector3f &

gyroBias

)

const

Definition at line 940 of file SoloGimbalEKF.cpp.

Here is the call graph for this function:

getQuat()

void SoloGimbalEKF::getQuat

(

Quaternion &

quat

)

const

Definition at line 959 of file SoloGimbalEKF.cpp.

getStatus()

bool SoloGimbalEKF::getStatus

(

void

)

const

Definition at line 965 of file SoloGimbalEKF.cpp.

Here is the call graph for this function:

predictCovariance()

void SoloGimbalEKF::predictCovariance ( )

private

Definition at line 195 of file SoloGimbalEKF.cpp.

Referenced by RunEKF().

Here is the call graph for this function:

Here is the caller graph for this function:

predictStates()

void SoloGimbalEKF::predictStates ( )

private

Definition at line 157 of file SoloGimbalEKF.cpp.

Referenced by RunEKF().

Here is the call graph for this function:

Here is the caller graph for this function:

readMagData()

void SoloGimbalEKF::readMagData ( )

private

Definition at line 665 of file SoloGimbalEKF.cpp.

Referenced by RunEKF().

Here is the call graph for this function:

Here is the caller graph for this function:

reset()

void SoloGimbalEKF::reset

(

)

Definition at line 42 of file SoloGimbalEKF.cpp.

Referenced by SoloGimbalEKF().

Here is the caller graph for this function:

RunEKF()

void SoloGimbalEKF::RunEKF	(	float	delta_time,
		const Vector3f &	delta_angles,
		const Vector3f &	delta_velocity,
		const Vector3f &	joint_angles
	)

Definition at line 57 of file SoloGimbalEKF.cpp.

Here is the call graph for this function:

setGyroBias()

void SoloGimbalEKF::setGyroBias

(

const Vector3f &

gyroBias

)

Definition at line 949 of file SoloGimbalEKF.cpp.

Member Data Documentation

_ahrs

const AP_AHRS_NavEKF& SoloGimbalEKF::_ahrs

private

Definition at line 103 of file SoloGimbalEKF.h.

Referenced by calcMagHeadingInnov(), fuseVelocity(), readMagData(), and RunEKF().

cosPhi

float SoloGimbalEKF::cosPhi

private

Definition at line 132 of file SoloGimbalEKF.h.

Referenced by calcMagHeadingInnov(), fuseCompass(), and RunEKF().

cosPsi

float SoloGimbalEKF::cosPsi

private

Definition at line 137 of file SoloGimbalEKF.h.

Referenced by calcMagHeadingInnov(), fuseCompass(), and RunEKF().

cosTheta

float SoloGimbalEKF::cosTheta

private

Definition at line 133 of file SoloGimbalEKF.h.

Referenced by calcMagHeadingInnov(), fuseCompass(), and RunEKF().

Cov

float SoloGimbalEKF::Cov[9][9]

private

Definition at line 125 of file SoloGimbalEKF.h.

Referenced by fixCovariance(), fuseCompass(), fuseVelocity(), predictCovariance(), reset(), and RunEKF().

delAng

Vector3f SoloGimbalEKF::delAng

Definition at line 118 of file SoloGimbalEKF.h.

delVel

Vector3f SoloGimbalEKF::delVel

Definition at line 119 of file SoloGimbalEKF.h.

dtIMU

float SoloGimbalEKF::dtIMU

private

Definition at line 142 of file SoloGimbalEKF.h.

Referenced by getDebug(), getGyroBias(), predictCovariance(), predictStates(), reset(), RunEKF(), and setGyroBias().

FiltInit

bool SoloGimbalEKF::FiltInit

private

Definition at line 130 of file SoloGimbalEKF.h.

Referenced by reset(), and RunEKF().

gPhi

float SoloGimbalEKF::gPhi

Definition at line 120 of file SoloGimbalEKF.h.

gPsi

float SoloGimbalEKF::gPsi

Definition at line 121 of file SoloGimbalEKF.h.

gSense

struct { ... } SoloGimbalEKF::gSense

Referenced by predictCovariance(), predictStates(), reset(), and RunEKF().

gTheta

float SoloGimbalEKF::gTheta

Definition at line 122 of file SoloGimbalEKF.h.

imuSampleTime_ms

uint32_t SoloGimbalEKF::imuSampleTime_ms

private

Definition at line 141 of file SoloGimbalEKF.h.

Referenced by RunEKF().

innovationIncrement

float SoloGimbalEKF::innovationIncrement

private

Definition at line 145 of file SoloGimbalEKF.h.

Referenced by calcMagHeadingInnov(), and reset().

lastInnovation

float SoloGimbalEKF::lastInnovation

private

Definition at line 146 of file SoloGimbalEKF.h.

Referenced by calcMagHeadingInnov(), and reset().

lastMagUpdate

uint32_t SoloGimbalEKF::lastMagUpdate

private

Definition at line 138 of file SoloGimbalEKF.h.

Referenced by readMagData(), and reset().

magData

Vector3f SoloGimbalEKF::magData

private

Definition at line 139 of file SoloGimbalEKF.h.

Referenced by calcMagHeadingInnov(), fuseCompass(), and readMagData().

newDataMag

bool SoloGimbalEKF::newDataMag

private

Definition at line 128 of file SoloGimbalEKF.h.

Referenced by readMagData(), and RunEKF().

sinPhi

float SoloGimbalEKF::sinPhi

private

Definition at line 134 of file SoloGimbalEKF.h.

Referenced by calcMagHeadingInnov(), fuseCompass(), and RunEKF().

sinPsi

float SoloGimbalEKF::sinPsi

private

Definition at line 136 of file SoloGimbalEKF.h.

Referenced by calcMagHeadingInnov(), fuseCompass(), and RunEKF().

sinTheta

float SoloGimbalEKF::sinTheta

private

Definition at line 135 of file SoloGimbalEKF.h.

Referenced by calcMagHeadingInnov(), fuseCompass(), and RunEKF().

StartTime_ms

uint32_t SoloGimbalEKF::StartTime_ms

private

Definition at line 129 of file SoloGimbalEKF.h.

Referenced by getStatus(), reset(), and RunEKF().

state

struct SoloGimbalEKF::state_elements & SoloGimbalEKF::state

private

Referenced by alignHeading(), fuseCompass(), fuseVelocity(), getDebug(), getGyroBias(), getQuat(), predictCovariance(), predictStates(), RunEKF(), and setGyroBias().

states

Vector13 SoloGimbalEKF::states

private

Definition at line 108 of file SoloGimbalEKF.h.

Referenced by fuseCompass(), fuseVelocity(), and reset().

TiltCorrection

float SoloGimbalEKF::TiltCorrection

private

Definition at line 127 of file SoloGimbalEKF.h.

Referenced by fuseVelocity(), getDebug(), reset(), and RunEKF().

Tsn

Matrix3f SoloGimbalEKF::Tsn

private

Definition at line 126 of file SoloGimbalEKF.h.

Referenced by alignHeading(), calcMagHeadingInnov(), and predictStates().

var_info

const AP_Param::GroupInfo SoloGimbalEKF::var_info

static

Initial value:

= {
    AP_GROUPEND
}

Definition at line 100 of file SoloGimbalEKF.h.

Referenced by SoloGimbalEKF().

YawAligned

bool SoloGimbalEKF::YawAligned

private

Definition at line 131 of file SoloGimbalEKF.h.

Referenced by fuseVelocity(), getStatus(), predictCovariance(), and RunEKF().

---

The documentation for this class was generated from the following files:

• libraries/AP_Mount/SoloGimbalEKF.h
• libraries/AP_Mount/SoloGimbalEKF.cpp