UnscentedKalmanFilter.cpp

Go to the documentation of this file.

/*
 * This file is part of ArmarX.
 *
 * ArmarX is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License version 2 as
 * published by the Free Software Foundation.
 *
 * ArmarX is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program. If not, see <http://www.gnu.org/licenses/>.
 *
 * @package    ROBDEKON
 * @author     Christoph Pohl ( christoph dot pohl at kit dot edu )
 * @date       16.03.21
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 *
 * Code adapted to C++ from: https://github.com/CAOR-MINES-ParisTech/ukfm
 * See also:
 *      - https://arxiv.org/pdf/2002.00878.pdf
 *      - https://ieeexplore.ieee.org/document/8206066/
 */
 
#include "UnscentedKalmanFilter.h"
 
#include <complex>
#include <utility>
 
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
 
template <typename SystemModelT>
UnscentedKalmanFilter<SystemModelT>::UnscentedKalmanFilter(const PropCovT& Q,
                                                           ObsCovT R,
                                                           const AlphaT& alpha,
                                                           StateT state0,
                                                           StateCovT P0) :
    Q_(Q),
    chol_Q_(Q.llt().matrixL().transpose()),
    R_(std::move(R)),
    alpha_(alpha),
    state_(std::move(state0)),
    P_(std::move(P0)),
    weights_(alpha)
{
}
 
template <typename SystemModelT>
void
UnscentedKalmanFilter<SystemModelT>::propagation(const ControlT& omega, FloatT dt)
{
    StateCovT P = P_ + eps * StateCovT::Identity();
    ARMARX_CHECK(P.allFinite());
    ControlNoiseT w = ControlNoiseT::Zero();
    // update mean
    StateT state_after = SystemModelT::propagationFunction(state_, omega, w, dt);
 
    // set sigma points (Xi in paper when on Manifold, sigma in Tangent Space)
    Eigen::LLT<StateCovT> llt = P.llt();
    if (llt.info() != Eigen::ComputationInfo::Success)
    {
        P = calculateClosestPosSemidefMatrix(P);
        P += eps * StateCovT::Identity();
        llt = P.llt();
        ARMARX_CHECK(llt.info() == Eigen::ComputationInfo::Success);
    }
    StateCovT xi_j_before =
        weights_.state.sqrt_d_lambda * llt.matrixL().transpose().toDenseMatrix();
    Eigen::Matrix<FloatT, 2 * dim::state, dim::state> xi_j_after;
    Eigen::Matrix<FloatT, 2 * dim::control, dim::state> w_j_after;
 
    auto propagate_and_retract = [&omega, &dt, &state_after](long row,
                                                             long offset,
                                                             const StateT& state,
                                                             const ControlNoiseT& noise,
                                                             auto& xi_j) -> void
    {
        const StateT sig_j_after = SystemModelT::propagationFunction(state, omega, noise, dt);
        xi_j.row(offset + row) = SystemModelT::inverseRetraction(state_after, sig_j_after);
    };
 
    auto calculate_covariance = [](FloatT wj, FloatT w0, auto& xi_j) -> StateCovT
    {
        SigmaPointsT xi_0 = wj * xi_j.colwise().sum();
        xi_j.rowwise() -= xi_0.transpose();
        StateCovT cov_after = wj * xi_j.transpose() * xi_j + w0 * xi_0 * xi_0.transpose();
        return cov_after;
    };
 
    for (long j = 0; j < dim::state; j++)
    {
        const SigmaPointsT sigma = xi_j_before.row(j);
        const StateT sig_j_plus = SystemModelT::retraction(state_, sigma);
        const StateT sig_j_minus = SystemModelT::retraction(state_, -sigma);
        propagate_and_retract(j, 0, sig_j_plus, w, xi_j_after);
        propagate_and_retract(j, dim::state, sig_j_minus, w, xi_j_after);
    }
 
    StateCovT P_after = calculate_covariance(weights_.state.wj, weights_.state.w0, xi_j_after);
    ARMARX_CHECK(P_after.allFinite());
    for (long j = 0; j < dim::control; j++)
    {
        const ControlNoiseT w_plus = weights_.control.sqrt_d_lambda * chol_Q_.row(j);
        const ControlNoiseT w_minus = -weights_.control.sqrt_d_lambda * chol_Q_.row(j);
        propagate_and_retract(j, 0, state_, w_plus, w_j_after);
        propagate_and_retract(j, dim::control, state_, w_minus, w_j_after);
    }
 
    StateCovT Q = calculate_covariance(weights_.control.wj, weights_.control.w0, w_j_after);
    ARMARX_CHECK(Q.allFinite());
    StateCovT new_P = P_after + Q;
    P_ = std::move((new_P + new_P.transpose()) / 2.0f);
    state_ = std::move(state_after);
}
 
template <typename SystemModelT>
void
UnscentedKalmanFilter<SystemModelT>::update(const ObsT& y)
{
    StateCovT P = P_ + eps * StateCovT::Identity();
    ARMARX_CHECK(P.allFinite());
    StateCovT xi_j = weights_.state.sqrt_d_lambda * P.llt().matrixL().transpose().toDenseMatrix();
    ARMARX_CHECK(P.llt().info() == Eigen::ComputationInfo::Success);
    ARMARX_CHECK(xi_j.allFinite());
    Eigen::Matrix<FloatT, 2 * dim::state, dim::obs> y_j =
        Eigen::Matrix<FloatT, 2 * dim::state, dim::obs>::Zero();
    ObsT y_hat = SystemModelT::observationFunction(state_);
 
    for (long j = 0; j < dim::state; j++)
    {
        const SigmaPointsT sigma = xi_j.row(j);
        const StateT sig_j_plus = SystemModelT::retraction(state_, sigma);
        const StateT sig_j_minus = SystemModelT::retraction(state_, -sigma);
        y_j.row(j) = SystemModelT::observationFunction(sig_j_plus);
        y_j.row(j + dim::state) = SystemModelT::observationFunction(sig_j_minus);
    }
    ARMARX_CHECK(y_j.allFinite());
    const ObsT y_mean =
        weights_.update.wm * y_hat + weights_.update.wj * y_j.colwise().sum().transpose();
    y_j.rowwise() -= y_mean.transpose();
    y_hat -= y_mean;
 
    const ObsCovT P_yy = weights_.update.w0 * y_hat * y_hat.transpose() +
                         weights_.update.wj * y_j.transpose() * y_j + R_;
    ARMARX_CHECK(P_yy.allFinite());
    const Eigen::Matrix<FloatT, dim::state, dim::obs> P_xiy =
        weights_.update.wj *
        (Eigen::Matrix<FloatT, dim::state, 2 * dim::state>() << xi_j, -xi_j).finished() *
        y_j /* + weights_.update.w0 * xi_0 * y_hat.transpose()*/;
    ARMARX_CHECK(P_xiy.allFinite());
    Eigen::Matrix<FloatT, dim::state, dim::obs> K =
        P_yy.colPivHouseholderQr().solve(P_xiy.transpose()).transpose();
    FloatT relative_error = (P_yy * K.transpose() - P_xiy.transpose()).norm() / P_xiy.norm();
    ARMARX_CHECK(relative_error < 1e-4);
    ARMARX_CHECK(K.allFinite());
    SigmaPointsT xi_plus = K * (y - y_mean);
    ARMARX_CHECK(xi_plus.allFinite());
    state_ = SystemModelT::retraction(state_, xi_plus);
    StateCovT new_P = P - K * (P_yy)*K.transpose();
 
    // make result symmetric
    P_ = calculateClosestPosSemidefMatrix(new_P);
}
 
template <typename SystemModelT>
const typename UnscentedKalmanFilter<SystemModelT>::StateT&
UnscentedKalmanFilter<SystemModelT>::getCurrentState() const
{
    return state_;
}
 
template <typename SystemModelT>
const typename UnscentedKalmanFilter<SystemModelT>::StateCovT&
UnscentedKalmanFilter<SystemModelT>::getCurrentStateCovariance() const
{
    return P_;
}
 
template <typename SystemModelT>
typename UnscentedKalmanFilter<SystemModelT>::StateCovT
UnscentedKalmanFilter<SystemModelT>::calculateClosestPosSemidefMatrix(
    const UnscentedKalmanFilter::StateCovT& cov)
{
    const StateCovT new_P = ((cov + cov.transpose()) / 2.0f);
    Eigen::EigenSolver<StateCovT> solver(new_P);
    Eigen::Matrix<FloatT, dim::state, dim::state> D = solver.eigenvalues().real().asDiagonal();
    const Eigen::Matrix<FloatT, dim::state, dim::state> V = solver.eigenvectors().real();
    D = D.cwiseMax(0);
 
    return (V * D * V.inverse());
}
 
template <typename SystemModelT>
UnscentedKalmanFilter<SystemModelT>::Weights::Weights(AlphaT alpha) :
    state(dim::state, alpha(0)), control(dim::control, alpha(1)), update(dim::state, alpha(2))
{
}
 
template <typename SystemModelT>
UnscentedKalmanFilter<SystemModelT>::Weights::W::W(size_t l, FloatT alpha)
{
    ARMARX_CHECK_GREATER(alpha, 0);
    FloatT m = (alpha * alpha - 1) * l;
    sqrt_d_lambda = std::sqrt(l + m);
    wj = 1 / (2 * (l + m));
    wm = m / (m + l);
    w0 = m / (m + l) + 3 - alpha * alpha;
}
 
template class UnscentedKalmanFilter<SystemModelSE3<float>>;
 
template class UnscentedKalmanFilter<SystemModelSE3<double>>;
 
template class UnscentedKalmanFilter<SystemModelSO3xR3<float>>;
 
template class UnscentedKalmanFilter<SystemModelSO3xR3<double>>;
 
///////////////////
 
 
template <typename SystemModelT>
UnscentedKalmanFilterWithoutControl<SystemModelT>::UnscentedKalmanFilterWithoutControl(
    ObsCovT R,
    const AlphaT& alpha,
    StateT state0,
    StateCovT P0) :
    R_(std::move(R)), alpha_(alpha), state_(std::move(state0)), P_(std::move(P0)), weights_(alpha)
{
}
 
template <typename SystemModelT>
void
UnscentedKalmanFilterWithoutControl<SystemModelT>::propagation(FloatT dt)
{
    StateCovT P = P_ + eps * StateCovT::Identity();
    ARMARX_CHECK(P.allFinite());
    // ControlNoiseT w = ControlNoiseT::Zero();
    // update mean
    StateT state_after = SystemModelT::propagationFunction(state_, dt);
 
    // set sigma points (Xi in paper when on Manifold, sigma in Tangent Space)
    Eigen::LLT<StateCovT> llt = P.llt();
    if (llt.info() != Eigen::ComputationInfo::Success)
    {
        P = calculateClosestPosSemidefMatrix(P);
        P += eps * StateCovT::Identity();
        llt = P.llt();
        ARMARX_CHECK(llt.info() == Eigen::ComputationInfo::Success);
    }
    StateCovT xi_j_before =
        weights_.state.sqrt_d_lambda * llt.matrixL().transpose().toDenseMatrix();
    Eigen::Matrix<FloatT, 2 * dim::state, dim::state> xi_j_after;
    Eigen::Matrix<FloatT, 2 * dim::control, dim::state> w_j_after;
 
    auto propagate_and_retract =
        [&dt, &state_after](long row, long offset, const StateT& state, auto& xi_j) -> void
    {
        const StateT sig_j_after = SystemModelT::propagationFunction(state, dt);
        xi_j.row(offset + row) = SystemModelT::inverseRetraction(state_after, sig_j_after);
    };
 
    auto calculate_covariance = [](FloatT wj, FloatT w0, auto& xi_j) -> StateCovT
    {
        SigmaPointsT xi_0 = wj * xi_j.colwise().sum();
        xi_j.rowwise() -= xi_0.transpose();
        StateCovT cov_after = wj * xi_j.transpose() * xi_j + w0 * xi_0 * xi_0.transpose();
        return cov_after;
    };
 
    for (long j = 0; j < dim::state; j++)
    {
        const SigmaPointsT sigma = xi_j_before.row(j);
        const StateT sig_j_plus = SystemModelT::retraction(state_, sigma);
        const StateT sig_j_minus = SystemModelT::retraction(state_, -sigma);
        propagate_and_retract(j, 0, sig_j_plus, xi_j_after);
        propagate_and_retract(j, dim::state, sig_j_minus, xi_j_after);
    }
 
    StateCovT P_after = calculate_covariance(weights_.state.wj, weights_.state.w0, xi_j_after);
    ARMARX_CHECK(P_after.allFinite());
    // for (long j = 0; j < dim::control; j++)
    // {
    //     const ControlNoiseT w_plus = weights_.control.sqrt_d_lambda * chol_Q_.row(j);
    //     const ControlNoiseT w_minus = -weights_.control.sqrt_d_lambda * chol_Q_.row(j);
    //     propagate_and_retract(j, 0, state_, w_plus, w_j_after);
    //     propagate_and_retract(j, dim::control, state_, w_minus, w_j_after);
    // }
 
    // StateCovT Q = calculate_covariance(weights_.control.wj, weights_.control.w0, w_j_after);
    // ARMARX_CHECK(Q.allFinite());
    StateCovT new_P = P_after; //  + Q;
    P_ = std::move((new_P + new_P.transpose()) / 2.0f);
    // P_ = new_P;
    state_ = std::move(state_after);
}
 
template <typename SystemModelT>
void
UnscentedKalmanFilterWithoutControl<SystemModelT>::update(const ObsT& y)
{
    StateCovT P = P_ + eps * StateCovT::Identity();
    ARMARX_CHECK(P.allFinite());
    StateCovT xi_j = weights_.state.sqrt_d_lambda * P.llt().matrixL().transpose().toDenseMatrix();
    ARMARX_CHECK(P.llt().info() == Eigen::ComputationInfo::Success);
    ARMARX_CHECK(xi_j.allFinite());
    Eigen::Matrix<FloatT, 2 * dim::state, dim::obs> y_j =
        Eigen::Matrix<FloatT, 2 * dim::state, dim::obs>::Zero();
    ObsT y_hat = SystemModelT::observationFunction(state_);
 
    for (long j = 0; j < dim::state; j++)
    {
        const SigmaPointsT sigma = xi_j.row(j);
        const StateT sig_j_plus = SystemModelT::retraction(state_, sigma);
        const StateT sig_j_minus = SystemModelT::retraction(state_, -sigma);
        y_j.row(j) = SystemModelT::observationFunction(sig_j_plus);
        y_j.row(j + dim::state) = SystemModelT::observationFunction(sig_j_minus);
    }
    ARMARX_CHECK(y_j.allFinite());
    const ObsT y_mean =
        weights_.update.wm * y_hat + weights_.update.wj * y_j.colwise().sum().transpose();
    y_j.rowwise() -= y_mean.transpose();
    y_hat -= y_mean;
 
    const ObsCovT P_yy = weights_.update.w0 * y_hat * y_hat.transpose() +
                         weights_.update.wj * y_j.transpose() * y_j + R_;
    ARMARX_CHECK(P_yy.allFinite());
    const Eigen::Matrix<FloatT, dim::state, dim::obs> P_xiy =
        weights_.update.wj *
        (Eigen::Matrix<FloatT, dim::state, 2 * dim::state>() << xi_j, -xi_j).finished() *
        y_j /* + weights_.update.w0 * xi_0 * y_hat.transpose()*/;
    ARMARX_CHECK(P_xiy.allFinite());
    Eigen::Matrix<FloatT, dim::state, dim::obs> K =
        P_yy.colPivHouseholderQr().solve(P_xiy.transpose()).transpose();
    FloatT relative_error = (P_yy * K.transpose() - P_xiy.transpose()).norm() / P_xiy.norm();
    ARMARX_CHECK(relative_error < 1e-4);
    ARMARX_CHECK(K.allFinite());
 
    const auto residual = y - y_mean;
 
    SigmaPointsT xi_plus = K * residual;
    ARMARX_CHECK(xi_plus.allFinite());
    state_ = SystemModelT::retraction(state_, xi_plus);
    StateCovT new_P = P - K * (P_yy)*K.transpose();
 
    // make result symmetric
    P_ = calculateClosestPosSemidefMatrix(new_P);
}
 
template <typename SystemModelT>
const typename UnscentedKalmanFilterWithoutControl<SystemModelT>::StateT&
UnscentedKalmanFilterWithoutControl<SystemModelT>::getCurrentState() const
{
    return state_;
}
 
template <typename SystemModelT>
const typename UnscentedKalmanFilterWithoutControl<SystemModelT>::StateCovT&
UnscentedKalmanFilterWithoutControl<SystemModelT>::getCurrentStateCovariance() const
{
    return P_;
}
 
template <typename SystemModelT>
typename UnscentedKalmanFilterWithoutControl<SystemModelT>::StateCovT
UnscentedKalmanFilterWithoutControl<SystemModelT>::calculateClosestPosSemidefMatrix(
    const UnscentedKalmanFilterWithoutControl::StateCovT& cov)
{
    const StateCovT new_P = ((cov + cov.transpose()) / 2.0f);
    Eigen::EigenSolver<StateCovT> solver(new_P);
    Eigen::Matrix<FloatT, dim::state, dim::state> D = solver.eigenvalues().real().asDiagonal();
    const Eigen::Matrix<FloatT, dim::state, dim::state> V = solver.eigenvectors().real();
    D = D.cwiseMax(0);
 
    return (V * D * V.inverse());
}
 
template <typename SystemModelT>
UnscentedKalmanFilterWithoutControl<SystemModelT>::Weights::Weights(AlphaT alpha) :
    state(dim::state, alpha(0)), update(dim::state, alpha(2))
{
}
 
template <typename SystemModelT>
UnscentedKalmanFilterWithoutControl<SystemModelT>::Weights::W::W(size_t l, FloatT alpha)
{
    ARMARX_CHECK_GREATER(alpha, 0);
    FloatT m = (alpha * alpha - 1) * l;
    sqrt_d_lambda = std::sqrt(l + m);
    wj = 1 / (2 * (l + m));
    wm = m / (m + l);
    w0 = m / (m + l) + 3 - alpha * alpha;
}