UnscentedKalmanFilter.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 <typename MatrixType, int UpLo = Eigen::Lower>
class Cholesky : public Eigen::LLT<MatrixType, UpLo>
{
public:
    Cholesky();
    Cholesky(const MatrixType& m);
    Cholesky& setIdentity();

    bool isIdentity() const;

    template <typename Derived>
    Cholesky& setL(const Eigen::MatrixBase<Derived>& matrix);

    template <typename Derived>
    Cholesky& setU(const Eigen::MatrixBase<Derived>& matrix);
};

template <size_t STATE_DIM, typename SCALAR>
struct UnscentedKalmanFilterSettings;

template <size_t STATE_DIM, size_t CONTROL_DIM, size_t OUTPUT_DIM, typename SCALAR = double>
class UnscentedKalmanFilter 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;

    static constexpr size_t SigmaPointCount = 2 * STATE_DIM + 1;

    template <size_t SIZE>
    using SigmaPoints = Eigen::Matrix<SCALAR, SIZE, SigmaPointCount>;

    template <size_t SIZE>
    using Covariance = Eigen::Matrix<SCALAR, SIZE, SIZE>;

    template <size_t SIZE>
    using CovarianceSquareRoot = Cholesky<Eigen::Matrix<SCALAR, SIZE, SIZE>>;

    UnscentedKalmanFilter(std::shared_ptr<SystemModelBase<STATE_DIM, CONTROL_DIM, SCALAR>> f,
        std::shared_ptr<LinearMeasurementModel<OUTPUT_DIM, STATE_DIM, SCALAR>> h,
        const state_vector_t& x0 = state_vector_t::Zero(),
        SCALAR alpha = SCALAR(1.0),
        SCALAR beta = SCALAR(2.0),
        SCALAR kappa = SCALAR(0.0),
        const ct::core::StateMatrix<STATE_DIM, SCALAR>& P0 = ct::core::StateMatrix<STATE_DIM, SCALAR>::Identity());

    UnscentedKalmanFilter(std::shared_ptr<SystemModelBase<STATE_DIM, CONTROL_DIM, SCALAR>> f,
        std::shared_ptr<LinearMeasurementModel<OUTPUT_DIM, STATE_DIM, SCALAR>> h,
        const UnscentedKalmanFilterSettings<STATE_DIM, SCALAR>& ukf_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;

    bool computeSigmaPoints();

    template <size_t SIZE>
    bool computeCovarianceFromSigmaPoints(const Eigen::Matrix<SCALAR, SIZE, 1>& mean,
        const SigmaPoints<SIZE>& sigmaPoints,
        const Covariance<SIZE>& noiseCov,
        Covariance<SIZE>& cov);

    bool computeKalmanGain(const ct::core::OutputVector<OUTPUT_DIM, SCALAR>& y,
        const SigmaPoints<OUTPUT_DIM>& sigmaMeasurementPoints,
        const Covariance<OUTPUT_DIM>& P_yy,
        Eigen::Matrix<SCALAR, STATE_DIM, OUTPUT_DIM>& K);

    bool updateStateCovariance(const Eigen::Matrix<SCALAR, STATE_DIM, OUTPUT_DIM>& K, const Covariance<OUTPUT_DIM>& P);

    const state_vector_t computeStatePrediction(const ct::core::ControlVector<CONTROL_DIM, SCALAR>& u,
        const ct::core::Time& dt,
        const ct::core::Time& t = 0);

    ct::core::OutputVector<OUTPUT_DIM, SCALAR> computeMeasurementPrediction(
        SigmaPoints<OUTPUT_DIM>& sigmaMeasurementPoints,
        const ct::core::Time& t = 0);

    void computeWeights();

    void computeSigmaPointTransition(const ct::core::ControlVector<CONTROL_DIM, SCALAR>& u,
        const ct::core::Time& dt,
        const ct::core::Time& t);

    void computeSigmaPointMeasurements(SigmaPoints<OUTPUT_DIM>& sigmaMeasurementPoints, const ct::core::Time& t = 0);

    template <size_t DIM>
    state_vector_t computePredictionFromSigmaPoints(const SigmaPoints<DIM>& sigmaPoints);

private:
    state_matrix_t P_;                                          
    Eigen::Matrix<SCALAR, SigmaPointCount, 1> sigmaWeights_m_;  
    Eigen::Matrix<SCALAR, SigmaPointCount, 1> sigmaWeights_c_;  
    SigmaPoints<STATE_DIM> sigmaStatePoints_;                   
    SCALAR alpha_;  
    SCALAR beta_;   
    SCALAR kappa_;  
    SCALAR gamma_;  
    SCALAR lambda_;  
};

}  // namespace optcon
}  // namespace ct