#include <DroneKalmanFilter.h>

Public Member Functions
void	addFakePTAMObservation (int time)
void	addPTAMObservation (TooN::Vector< 6 > trans, int time)
TooN::Vector< 6 >	backTransformPTAMObservation (TooN::Vector< 6 > obs)
void	clearPTAM ()
	DroneKalmanFilter (EstimationNode *node)
void	flushScalePairs ()
TooN::Vector< 6 >	getCurrentOffsets ()
TooN::Vector< 6 >	getCurrentPose ()
uga_tum_ardrone::filter_state	getCurrentPoseSpeed ()
TooN::Vector< 10 >	getCurrentPoseSpeedAsVec ()
TooN::Vector< 10 >	getCurrentPoseSpeedVariances ()
TooN::Vector< 6 >	getCurrentPoseVariances ()
TooN::Vector< 3 >	getCurrentScales ()
TooN::Vector< 3 >	getCurrentScalesForLog ()
int	getNumGoodPTAMObservations ()
uga_tum_ardrone::filter_state	getPoseAt (ros::Time t, bool useControlGains=true)
TooN::Vector< 10 >	getPoseAtAsVec (int timestamp, bool useControlGains=true)
float	getScaleAccuracy ()
bool	handleCommand (std::string s)
void	predictUpTo (int timestamp, bool consume=true, bool useControlGains=true)
void	release ()
void	reset ()
void	setCurrentScales (TooN::Vector< 3 > scales)
void	setPing (unsigned int navPing, unsigned int vidPing)
TooN::Vector< 3 >	transformPTAMObservation (double x, double y, double z)
TooN::Vector< 3 >	transformPTAMObservation (double x, double y, double z, double yaw)
TooN::Vector< 6 >	transformPTAMObservation (TooN::Vector< 6 > obs)
void	updateScaleXYZ (TooN::Vector< 3 > ptamDiff, TooN::Vector< 3 > imuDiff, TooN::Vector< 3 > OrgPtamPose)
	~DroneKalmanFilter (void)
Public Attributes
bool	allSyncLocked
float	c1
float	c2
float	c3
float	c4
float	c5
float	c6
float	c7
float	c8
std::deque < ardrone_autonomy::Navdata > *	navdataQueue
int	predictdUpToTimestamp
int	scalePairsIn
int	scalePairsOut
TooN::Vector< 3 >	scalingFixpoint
bool	useControl
bool	useNavdata
bool	usePTAM
bool	useScalingFixpoint
std::deque < geometry_msgs::TwistStamped > *	velQueue
float	yawDriftThreshold
float	zDriftThreshold
Static Public Attributes
static const int	base_delayControl = 50
static const int	base_delayVideo = 50
static const int	base_delayXYZ = 40
static int	delayControl = 100
static int	delayRPY = 0
static int	delayVideo = 75
static int	delayXYZ = 40
static pthread_mutex_t	filter_CS = PTHREAD_MUTEX_INITIALIZER
Private Member Functions
void	observeIMU_RPY (const ardrone_autonomy::Navdata *nav)
void	observeIMU_XYZ (const ardrone_autonomy::Navdata *nav)
void	observePTAM (TooN::Vector< 6 > pose)
void	predictInternal (geometry_msgs::Twist activeControlInfo, int timeSpanMicros, bool useControlGains=true)
void	sync_rpy (double roll_global, double pitch_global, double yaw_global)
void	sync_xyz (double x_global, double y_global, double z_global)
Private Attributes
bool	baselinesYValid
double	baselineY_IMU
double	baselineZ_Filter
double	baselineZ_IMU
double	initialScaleSet
TooN::Vector< 3 >	last_fused_pose
TooN::Vector< 3 >	last_PTAM_pose
long	last_yaw_droneTime
long	last_z_droneTime
double	last_z_heightDiff
double	last_z_IMU
long	last_z_packageID
double	lastdYaw
double	lastdZ
long	lastIMU_dronetime
long	lastIMU_RPY_dronetime
int	lastIMU_RPY_ID
long	lastIMU_XYZ_dronetime
int	lastIMU_XYZ_ID
bool	lastPosesValid
double	lastPredictedPitch
double	lastPredictedRoll
double	lastVXGain
double	lastVYGain
EstimationNode *	node
int	numGoodIMUObservations
int	numGoodPTAMObservations
bool	offsets_xyz_initialized
PFilter	pitch
double	pitch_offset
long	predictedUpToDroneTime
int	predictedUpToTotal
PFilter	roll
double	roll_offset
double	rp_offset_framesContributed
double	scale_from_xy
double	scale_from_z
bool	scale_xyz_initialized
std::vector< ScaleStruct > *	scalePairs
int	timestampYawBaselineFrom
PVFilter	x
double	x_offset
double	xy_scale
double	xyz_sum_IMUxIMU
double	xyz_sum_PTAMxIMU
double	xyz_sum_PTAMxPTAM
PVFilter	y
double	y_offset
PVFilter	yaw
double	yaw_offset
PVFilter	z
double	z_offset
double	z_scale