Skip to content

Commit

Permalink
Extended Kalman Filter
Browse files Browse the repository at this point in the history 
Try 1 code development
  • Loading branch information
@pasoppido
pasoppido committed Jun 5, 2018
1 parent e3e39b1 commit 7926c12
Show file tree
Hide file tree
Showing 2 changed files with 99 additions and 49 deletions.
9 changes: 9 additions & 0 deletions include/teamsannio_med_control/extendedKalmanFilter.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -42,9 +42,18 @@ class ExtendedKalmanFilter {
private:

EigenOdometry odometry_private_;



ros::NodeHandle n11_;
ros::Timer t11_;

void ExtendedKalmanFilter::CallbackEstimate(const ros::TimerEvent& event)

void Quaternion2Euler(double* roll, double* pitch, double* yaw) const;
void SetOdometry(const EigenOdometry& odometry);
void ExtendedKalmanFilter::correct(Eigen::Matrix6f* xe, Eigen::Matrix6f* pe);
void ExtendedKalmanFilter::predict(Eigen::Vector6f* xp, Eigen::Matrix6f* P);

};

Expand Down
139 changes: 90 additions & 49 deletions src/library/extendedKalmanFilter.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -30,10 +30,17 @@
#include <mav_msgs/conversions.h>
#include <ros/console.h>

#define TsP 10e-3 /* Position control sampling time */



namespace teamsannio_med_control {

ExtendedKalmanFilter::ExtendedKalmanFilter() {}
ExtendedKalmanFilter::ExtendedKalmanFilter() {

timer11_ = n11_.createTimer(ros::Duration(TsP), &ExtendedKalmanFilter::CallbackEstimate, this, false, true);

}

ExtendedKalmanFilter::~ExtendedKalmanFilter() {}

Expand All @@ -56,9 +63,6 @@ void ExtendedKalmanFilter::Estimator(state_t *state_, EigenOdometry* odometry_){
state_->linearVelocity.y = odometry_private_.velocity[1];
state_->linearVelocity.z = odometry_private_.velocity[2];

double roll, pitch, yaw;
Quaternion2Euler(&roll, &pitch, &yaw);

state_->attitude.roll = roll;
state_->attitude.pitch = pitch;
state_->attitude.yaw = yaw;
Expand All @@ -69,88 +73,116 @@ void ExtendedKalmanFilter::Estimator(state_t *state_, EigenOdometry* odometry_){

}

void ExtendedKalmanFilter::Quaternion2Euler(double* roll, double* pitch, double* yaw) const {


}
void ExtendedKalmanFilter::SetVehicleParameters(){

m_ = vehicle_parameters_.mass_;
g_ = vehicle_parameters_.gravity_;
Ix_ = vehicle_parameters_.inertia_(0,0);
Iy_ = vehicle_parameters_.inertia_(1,1);
Iz_ = vehicle_parameters_.inertia_(2,2);

}

void Ekf::Predict(double* u_T, double* hatx, double* Pp)
{
void ExtendedKalmanFilter::Quaternion2Euler(double* roll, double* pitch, double* yaw) const {
assert(roll);
assert(pitch);
assert(yaw);

double x, y, z, dx, dy, dz, w;
double x, y, z, w;
x = odometry_private_.orientation.x();
y = odometry_private_.orientation.y();
z = odometry_private_.orientation.z();
dx = odometry_private_.velocity[0];
dy = odometry_private_.velocity[1];
dz = odometry_private_.velocity[2];

w = odometry_private_.orientation.w();

tf::Quaternion q(x, y, z, w);
tf::Matrix3x3 m(q);
m.getRPY(*roll, *pitch, *yaw);

}

void ExtendedKalmanFilter::predict(Eigen::Vector6f* xp, Eigen::Matrix6f* P){
assert(xp);
assert(P);

double x, y, z, dx, dy, dz;
x = odometry_private_.position[0];
y = odometry_private_.position[1];
z = odometry_private_.position[2];
dx = odometry_private_.velocity[0];
dy = odometry_private_.velocity[1];
dz = odometry_private_.velocity[2];

phi = state.attitude.roll;
theta = state.attitude.pitch;
psi = state.attitude.yaw;


dot_x = (cos(theta) * cos(psi) * dx) +
( ( (sin(phi) * sin(theta) * cos(psi) ) - ( cos(phi) * sin(psi) ) ) * dy) +
( ( (cos(phi) * sin(theta) * cos(psi) ) + ( sin(phi) * sin(psi) ) ) * dz);

dot_y = (cos(theta) * sin(psi) * dx) +
( ( (sin(phi) * sin(theta) * sin(psi) ) + ( cos(phi) * cos(psi) ) ) * dy) +
( ( (cos(phi) * sin(theta) * sin(psi) ) - ( sin(phi) * cos(psi) ) ) * dz);

dot_z = (-sin(theta) * dx) + ( sin(phi) * cos(theta) * dy) +
(cos(phi) * cos(theta) * dz);


double u_x, u_y, u_T, u_Terr;
PositionController::PosController(&u_x, &u_y, &u_T, &u_Terr);

// Non linear Position model discretized with forward Euler
x = x + Tsp_ * dx;
y = y + Tsp_ * dy;
z = z + Tsp_ * dz;
dx = dx + Tsp_ * (*u_T/m_ * (cos(state_.attitude.yaw) * sin(state_.attitude.pitch) * cos(state_.attitude.roll) + sin(state_.attitude.yaw) * sin(state_.attitude.pitch)));
dy = dy + Tsp_ * (*u_T/m_ * (sin(state_.attitude.yaw) * sin(state_.attitude.pitch) * cos(state_.attitude.roll) - cos(state_.attitude.yaw) * sin(state_.attitude.roll)));
dx = dx + Tsp_ * (-g_ + *u_T/m_ * (cos(state_.attitude.pitch) * cos(state_.attitude.roll));
x = x + Tsp_ * dot_x;
y = y + Tsp_ * dot_y;
z = z + Tsp_ * dot_z;
dot_x = dot_x + Tsp_ * (*u_T/m_ * (cos(psi) * sin(theta) * cos(phi) + sin(psi) * sin(theta)));
dot_y = dot_y + Tsp_ * (*u_T/m_ * (sin(psi) * sin(theta) * cos(phi) - cos(psi) * sin(phi)));
dot_z = dot_z + Tsp_ * (-g_ + *u_T/m_ * (cos(theta) * cos(phi));

// Jacobian Matrix
Matrix6f A;
Eigen::Matrix6f A;

A << 1, 0, 0, Tsp_, 0, 0,
0, 1, 0, 0, Tsp_, 0,
0, 0, 1, 0, 0, Tsp_,
0, 0, 1, 0, 0, Tsp_,
0, 0, 0, 1, 0, 0,
0, 0, 0, 0, 1, 0,
0, 0, 0, 0, 0, 1;

Matrix6f Qp;

Qp << 0.000001, 0, 0, 0, 0, 0,
0, 0.000001, 0, 0, 0, 0,
0, 0, 0.000001, 0, 0, 0,
0, 0, 0, 0.000001, 0, 0,
0, 0, 0, 0, 0.000001, 0,
0, 0, 0, 0, 0, 0.000001;
Eigen::Matrix6f Qp;

Qp << 1e-6, 0, 0, 0, 0, 0,
0, 1e-6, 0, 0, 0, 0,
0, 0, 1e-6, 0, 0, 0,
0, 0, 0, 1e-6, 0, 0,
0, 0, 0, 0, 1e-6, 0,
0, 0, 0, 0, 0, 1e-6;

Qp = pow(Qp,2);

// Prediction error Matrix
P = A * Pp * A.transpose() + Qp;
Eigen::MatrixXd P = A * P * A.transpose() + Qp;

VectorXd xp(6);
Eigen::VectorXd xp(6);

xp(1) = x;
xp(2) = y;
xp(3) = z;
xp(4) = dx;
xp(5) = dy;
xp(6) = dz;
xp(4) = dot_x;
xp(5) = dot_y;
xp(6) = dot_z;

}


void EKF::Correct(double* xp, double* P){
void ExtendedKalmanFilter::correct(Eigen::Matrix6f* xe, Eigen::Matrix6f* pe){

double x, y, z, dx, dy, dz, w;
x = odometry_private_.orientation.x();
y = odometry_private_.orientation.y();
z = odometry_private_.orientation.z();
dx = odometry_private_.velocity[0];
dy = odometry_private_.velocity[1];
dz = odometry_private_.velocity[2];
Eigen::Matrix6f P;
Eigen::Vector6f xp;
predict( &xp, &P);

Matrix6f Hp;
Eigen::Matrix6f Hp;

Hp << 1, 0, 0, 0, 0, 0,
0, 1, 0, 0, 0, 0,
Expand All @@ -159,7 +191,7 @@ void EKF::Correct(double* xp, double* P){
0, 0, 0, 0, 1, 0,
0, 0, 0, 0, 0, 1;

Matrix6f Rp;
Eigen::Matrix6f Rp;

Rp << 0.01, 0, 0, 0, 0, 0,
0, 0.01, 0, 0, 0, 0,
Expand All @@ -170,10 +202,19 @@ void EKF::Correct(double* xp, double* P){

Rp = pow(Rp,2);

K = P * Hp * (Hp.transpose() * P * Hp + Rp).inverse();
Eigen::Matrix6f K = P_ * Hp * (Hp.transpose() * P_ * Hp + Rp).inverse();

pe = P - K * Hp.transpose() * P;
xe = xp + K * (y - Hp * xp);
Eigen::Matrix6f *pe = P - K * Hp.transpose() * P_;
Eigen::Matrix6f *xe = xp_ + K * (y - Hp * xp_);


}

void ExtendedKalmanFilter::CallbackEstimate(const ros::TimerEvent& event){

predict(&xp, &P);
correct(&xp, &pe);
}


}

0 comments on commit 7926c12

Please sign in to comment.