FilterOF.cpp File Reference

#include "ros/ros.h"
#include <sensor_msgs/Imu.h>
#include <math.h>
#include "tf/transform_datatypes.h"
#include <Eigen/Eigen>
#include <Eigen/LU>
#include "px_comm/OpticalFlow.h"
#include "std_msgs/Float64.h"
#include "quad_PX4/KState.h"

Include dependency graph for FilterOF.cpp:

Go to the source code of this file.

Classes
struct	Az_str
Functions
void	AccZCallback (std_msgs::Float64 const &msg)
void	imuCallback (sensor_msgs::Imu const &imu)
int	main (int argc, char **argv)
void	PX4Callback (px_comm::OpticalFlow const &msg)
void	XYKalman_Init ()
double	XYKalman_Predict (double AccZ, double _dt)
double	XYKalman_Update (Eigen::Matrix< double, 2, 1 > Z)
Variables
Eigen::Matrix< double, 6, 6 >	A
Az_str	Az
bool	Az_init = false
Eigen::Matrix< double, 6, 2 >	B
Eigen::Matrix< double, 2, 6 >	C
Eigen::Matrix< double, 6, 2 >	K
bool	K_init = false
ros::Publisher	Kstate_pub
Eigen::Matrix< double, 2, 1 >	OF
Eigen::Matrix< double, 6, 6 >	P
tfScalar	pitch
Eigen::Matrix< double, 4, 4 >	Q
Eigen::Matrix< double, 2, 2 >	R
Eigen::Matrix< double, 2, 1 >	Resid
tfScalar	roll
Eigen::Matrix< double, 3, 3 >	RotMat
Eigen::Matrix< double, 3, 3 >	Rpitch
Eigen::Matrix< double, 3, 3 >	Rroll
Eigen::Matrix< double, 3, 3 >	Ryaw
Eigen::Matrix< double, 2, 1 >	U
Eigen::Matrix< double, 3, 1 >	VecAz
Eigen::Matrix< double, 3, 1 >	VecDir
Eigen::Matrix< double, 3, 1 >	VecG
Eigen::Matrix< double, 6, 4 >	W
Eigen::Matrix< double, 6, 1 >	Xhat
tfScalar	yaw

---

Function Documentation

void AccZCallback

(

std_msgs::Float64 const &

msg

)

Definition at line 133 of file FilterOF.cpp.

void imuCallback

(

sensor_msgs::Imu const &

imu

)

Definition at line 150 of file FilterOF.cpp.

int main	(	int	argc,
		char **	argv
	)

Definition at line 173 of file FilterOF.cpp.

void PX4Callback

(

px_comm::OpticalFlow const &

msg

)

Definition at line 156 of file FilterOF.cpp.

void XYKalman_Init

(

)

Definition at line 38 of file FilterOF.cpp.

double XYKalman_Predict	(	double	AccZ,
		double	_dt
	)

Definition at line 78 of file FilterOF.cpp.

double XYKalman_Update

(

Eigen::Matrix< double, 2, 1 >

Z

)

Definition at line 118 of file FilterOF.cpp.

---

Variable Documentation

Eigen::Matrix<double, 6, 6> A

Definition at line 27 of file FilterOF.cpp.

Az_str Az

Definition at line 24 of file FilterOF.cpp.

bool Az_init = false

Definition at line 13 of file FilterOF.cpp.

Eigen::Matrix<double, 6, 2> B

Definition at line 28 of file FilterOF.cpp.

Eigen::Matrix<double, 2, 6> C

Definition at line 33 of file FilterOF.cpp.

Eigen::Matrix<double, 6, 2> K

Definition at line 28 of file FilterOF.cpp.

bool K_init = false

Definition at line 13 of file FilterOF.cpp.

ros::Publisher Kstate_pub

Definition at line 15 of file FilterOF.cpp.

Eigen::Matrix<double, 2, 1> OF

Definition at line 30 of file FilterOF.cpp.

Eigen::Matrix<double, 6, 6> P

Definition at line 27 of file FilterOF.cpp.

tfScalar pitch

Definition at line 12 of file FilterOF.cpp.

Eigen::Matrix<double, 4, 4> Q

Definition at line 31 of file FilterOF.cpp.

Eigen::Matrix<double, 2, 2> R

Definition at line 34 of file FilterOF.cpp.

Eigen::Matrix<double, 2, 1> Resid

Definition at line 30 of file FilterOF.cpp.

tfScalar roll

Definition at line 12 of file FilterOF.cpp.

Eigen::Matrix<double, 3, 3> RotMat

Definition at line 35 of file FilterOF.cpp.

Eigen::Matrix<double, 3, 3> Rpitch

Definition at line 35 of file FilterOF.cpp.

Eigen::Matrix<double, 3, 3> Rroll

Definition at line 35 of file FilterOF.cpp.

Eigen::Matrix<double, 3, 3> Ryaw

Definition at line 35 of file FilterOF.cpp.

Eigen::Matrix<double, 2, 1> U

Definition at line 30 of file FilterOF.cpp.

Eigen::Matrix<double, 3, 1> VecAz

Definition at line 36 of file FilterOF.cpp.

Eigen::Matrix<double, 3, 1> VecDir

Definition at line 36 of file FilterOF.cpp.

Eigen::Matrix<double, 3, 1> VecG

Definition at line 36 of file FilterOF.cpp.

Eigen::Matrix<double, 6, 4> W

Definition at line 32 of file FilterOF.cpp.

Eigen::Matrix<double, 6, 1> Xhat

Definition at line 29 of file FilterOF.cpp.

tfScalar yaw

Definition at line 12 of file FilterOF.cpp.