ExtendedKalmanFilter.h

Go to the documentation of this file.

/**********************************************************************************************************************
This file is part of the Control Toolbox (https://github.com/ethz-adrl/control-toolbox), copyright by ETH Zurich.
Licensed under the BSD-2 license (see LICENSE file in main directory)
**********************************************************************************************************************/

#pragma once

#include "EstimatorBase.h"

namespace ct {
namespace optcon {

template <size_t STATE_DIM, typename SCALAR>
struct ExtendedKalmanFilterSettings;

template <size_t STATE_DIM, size_t CONTROL_DIM, size_t OUTPUT_DIM, typename SCALAR = double>
class ExtendedKalmanFilter final : public EstimatorBase<STATE_DIM, CONTROL_DIM, OUTPUT_DIM, SCALAR>
{
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW

    using Base = EstimatorBase<STATE_DIM, CONTROL_DIM, OUTPUT_DIM, SCALAR>;
    using typename Base::control_vector_t;
    using typename Base::output_matrix_t;
    using typename Base::output_vector_t;
    using typename Base::state_matrix_t;
    using typename Base::state_vector_t;

    ExtendedKalmanFilter(std::shared_ptr<SystemModelBase<STATE_DIM, CONTROL_DIM, SCALAR>> f,
        std::shared_ptr<LinearMeasurementModel<OUTPUT_DIM, STATE_DIM, SCALAR>> h,
        const state_matrix_t& Q,
        const output_matrix_t& R,
        const state_vector_t& x0 = state_vector_t::Zero(),
        const state_matrix_t& P0 = state_matrix_t::Zero());

    ExtendedKalmanFilter(std::shared_ptr<SystemModelBase<STATE_DIM, CONTROL_DIM, SCALAR>> f,
        std::shared_ptr<LinearMeasurementModel<OUTPUT_DIM, STATE_DIM, SCALAR>> h,
        const ExtendedKalmanFilterSettings<STATE_DIM, SCALAR>& ekf_settings);

    const state_vector_t& predict(const control_vector_t& u,
        const ct::core::Time& dt,
        const ct::core::Time& t) override;

    const state_vector_t& update(const output_vector_t& y, const ct::core::Time& dt, const ct::core::Time& t) override;

    // return current covariance matrix
    const state_matrix_t& getCovarianceMatrix();

    void setQ(const state_matrix_t& Q) { Q_ = Q; }
    void setR(const output_matrix_t& R) { R_ = R; }
protected:
    state_matrix_t Q_;

    output_matrix_t R_;

    state_matrix_t P_;
};

}  // namespace optcon
}  // namespace ct