#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"
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 |
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.
Eigen::Matrix<double, 6, 6> A |
Definition at line 27 of file FilterOF.cpp.
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.
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.
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.
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.
Definition at line 12 of file FilterOF.cpp.