PosVelEKF.cpp File Reference

#include "PosVelEKF.h"
#include <math.h>
#include <string.h>

Include dependency graph for PosVelEKF.cpp:

Go to the source code of this file.

Macros
#define	POSVELEKF_POS_CALC_NIS(__P, __R, __X, __Z, __RET_NIS)   __RET_NIS = ((-__X[0] + __Z)*(-__X[0] + __Z))/(__P[0] + __R);
#define	POSVELEKF_POS_CALC_STATE(__P, __R, __X, __Z, __RET_STATE)
#define	POSVELEKF_POS_CALC_COV(__P, __R, __X, __Z, __RET_COV)
#define	POSVELEKF_PREDICTION_CALC_STATE(__P, __DT, __DV, __DV_NOISE, __X, __RET_STATE)   __RET_STATE[0] = __DT*__X[1] + __X[0]; __RET_STATE[1] = __DV + __X[1];
#define	POSVELEKF_PREDICTION_CALC_COV(__P, __DT, __DV, __DV_NOISE, __X, __RET_COV)
#define	POSVELEKF_VEL_CALC_NIS(__P, __R, __X, __Z, __RET_NIS)   __RET_NIS = ((-__X[1] + __Z)*(-__X[1] + __Z))/(__P[2] + __R);
#define	POSVELEKF_VEL_CALC_STATE(__P, __R, __X, __Z, __RET_STATE)
#define	POSVELEKF_VEL_CALC_COV(__P, __R, __X, __Z, __RET_COV)

Macro Definition Documentation

POSVELEKF_POS_CALC_COV

#define POSVELEKF_POS_CALC_COV	(		__P,
			__R,
			__X,
			__Z,
			__RET_COV
	)

Value:

__RET_COV[0] = ((__P[0])*(__P[0]))*__R/((__P[0] + __R)*(__P[0] + __R)) + __P[0]*((-__P[0]/(__P[0] + \
__R) + 1)*(-__P[0]/(__P[0] + __R) + 1)); __RET_COV[1] = __P[0]*__P[1]*__R/((__P[0] + __R)*(__P[0] + \
__R)) - __P[0]*__P[1]*(-__P[0]/(__P[0] + __R) + 1)/(__P[0] + __R) + __P[1]*(-__P[0]/(__P[0] + __R) + \
1); __RET_COV[2] = ((__P[1])*(__P[1]))*__R/((__P[0] + __R)*(__P[0] + __R)) - \
((__P[1])*(__P[1]))/(__P[0] + __R) - __P[1]*(-__P[0]*__P[1]/(__P[0] + __R) + __P[1])/(__P[0] + __R) + \
__P[2];

Definition at line 12 of file PosVelEKF.cpp.

Referenced by PosVelEKF::fusePos().

POSVELEKF_POS_CALC_NIS

#define POSVELEKF_POS_CALC_NIS	(		__P,
			__R,
			__X,
			__Z,
			__RET_NIS
	)		__RET_NIS = ((-__X[0] + __Z)*(-__X[0] + __Z))/(__P[0] + __R);

Definition at line 5 of file PosVelEKF.cpp.

Referenced by PosVelEKF::getPosNIS().

POSVELEKF_POS_CALC_STATE

#define POSVELEKF_POS_CALC_STATE	(		__P,
			__R,
			__X,
			__Z,
			__RET_STATE
	)

Value:

__RET_STATE[0] = __P[0]*(-__X[0] + __Z)/(__P[0] + __R) + __X[0]; __RET_STATE[1] = __P[1]*(-__X[0] + \
__Z)/(__P[0] + __R) + __X[1];

Definition at line 8 of file PosVelEKF.cpp.

Referenced by PosVelEKF::fusePos().

POSVELEKF_PREDICTION_CALC_COV

#define POSVELEKF_PREDICTION_CALC_COV	(		__P,
			__DT,
			__DV,
			__DV_NOISE,
			__X,
			__RET_COV
	)

Value:

__RET_COV[0] = __DT*__P[1] + __DT*(__DT*__P[2] + __P[1]) + __P[0]; __RET_COV[1] = __DT*__P[2] + \
__P[1]; __RET_COV[2] = ((__DV_NOISE)*(__DV_NOISE)) + __P[2];

Definition at line 23 of file PosVelEKF.cpp.

Referenced by PosVelEKF::predict().

POSVELEKF_PREDICTION_CALC_STATE

#define POSVELEKF_PREDICTION_CALC_STATE	(		__P,
			__DT,
			__DV,
			__DV_NOISE,
			__X,
			__RET_STATE
	)		__RET_STATE[0] = __DT*__X[1] + __X[0]; __RET_STATE[1] = __DV + __X[1];

Definition at line 20 of file PosVelEKF.cpp.

Referenced by PosVelEKF::predict().

POSVELEKF_VEL_CALC_COV

#define POSVELEKF_VEL_CALC_COV	(		__P,
			__R,
			__X,
			__Z,
			__RET_COV
	)

Value:

__RET_COV[0] = __P[0] + ((__P[1])*(__P[1]))*__R/((__P[2] + __R)*(__P[2] + __R)) - \
((__P[1])*(__P[1]))/(__P[2] + __R) - __P[1]*(-__P[1]*__P[2]/(__P[2] + __R) + __P[1])/(__P[2] + __R); \
__RET_COV[1] = __P[1]*__P[2]*__R/((__P[2] + __R)*(__P[2] + __R)) + (-__P[2]/(__P[2] + __R) + \
1)*(-__P[1]*__P[2]/(__P[2] + __R) + __P[1]); __RET_COV[2] = ((__P[2])*(__P[2]))*__R/((__P[2] + \
__R)*(__P[2] + __R)) + __P[2]*((-__P[2]/(__P[2] + __R) + 1)*(-__P[2]/(__P[2] + __R) + 1));

Definition at line 34 of file PosVelEKF.cpp.

Referenced by PosVelEKF::fuseVel().

POSVELEKF_VEL_CALC_NIS

#define POSVELEKF_VEL_CALC_NIS	(		__P,
			__R,
			__X,
			__Z,
			__RET_NIS
	)		__RET_NIS = ((-__X[1] + __Z)*(-__X[1] + __Z))/(__P[2] + __R);

Definition at line 27 of file PosVelEKF.cpp.

POSVELEKF_VEL_CALC_STATE

#define POSVELEKF_VEL_CALC_STATE	(		__P,
			__R,
			__X,
			__Z,
			__RET_STATE
	)

Value:

__RET_STATE[0] = __P[1]*(-__X[1] + __Z)/(__P[2] + __R) + __X[0]; __RET_STATE[1] = __P[2]*(-__X[1] + \
__Z)/(__P[2] + __R) + __X[1];

Definition at line 30 of file PosVelEKF.cpp.

Referenced by PosVelEKF::fuseVel().