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