ekf.hpp

Go to the documentation of this file.

/*********************************************************************
*  Copyright (c) 2017 Robert Bosch GmbH.
*  All rights reserved.
*
*  Licensed under the Apache License, Version 2.0 (the "License");
*  you may not use this file except in compliance with the License.
*  You may obtain a copy of the License at
*
*      http://www.apache.org/licenses/LICENSE-2.0
*
*  Unless required by applicable law or agreed to in writing, software
*  distributed under the License is distributed on an "AS IS" BASIS,
*  WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
*  See the License for the specific language governing permissions and
*  limitations under the License.
* *********************************************************************/
 
#ifndef EKF_HPP
#define EKF_HPP
 
#include <Eigen/Dense>
 
#include "steering_functions/steering_functions.hpp"
 
namespace steering
{
 
typedef Eigen::Matrix<double, 2, 2> Matrix2d;
typedef Eigen::Matrix<double, 3, 3> Matrix3d;
typedef Eigen::Matrix<double, 3, 2> Matrix32d;
typedef Eigen::Matrix<double, 2, 3> Matrix23d;
 
class EKF
{
public:
  EKF();
 
  void set_parameters(const Motion_Noise &motion_noise, const Measurement_Noise &measurement_noise,
                      const Controller &_controller);
 
  Matrix3d covariance_to_eigen(const double covariance[16]) const;
 
  void eigen_to_covariance(const Matrix3d &covariance_eigen, double covariance[16]) const;
 
  void get_motion_jacobi(const State &state, const Control &control, double integration_step, Matrix3d &F_x,
                         Matrix32d &F_u) const;
 
  Matrix3d get_observation_jacobi() const;
 
  Matrix2d get_motion_covariance(const State &state, const Control &control, double integration_step) const;
 
  Matrix3d get_observation_covariance() const;
 
  Matrix23d get_controller_gain(const Control &control) const;
 
  Matrix3d get_rotation_matrix(double angle) const;
 
  void predict(const State_With_Covariance &state, const Control &control, double integration_step,
               State_With_Covariance &state_pred) const;
 
  void update(const State_With_Covariance &state_pred, State_With_Covariance &state_corr) const;
 
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
 
private:
  Motion_Noise motion_noise_;
 
  Measurement_Noise measurement_noise_;
 
  Controller controller_;
 
  Matrix3d I_;
};
 
} // namespace steering
 
#endif