HumanSystemModel.h

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/>.
 *
 * @author     Tobias Gröger ( tobias dot groeger at student dot kit dot edu )
 * @author     Corvin Navarro Ecker ( corvin dot ecker at student dot kit dot edu )
 * @date       2022
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
#pragma once
 
#include <manif/SE2.h> // IWYU pragma: keep
#include <manif/SO2.h> // IWYU pragma: keep
 
namespace armarx::navigation::human::kalman_filter
{
    //----------- SO2xR2 -----------------
 
    template <typename floatT>
    struct StateSO2xR2
    {
        manif::SO2<floatT> orientation;
        Eigen::Matrix<floatT, 2, 1> position;
    };
 
    template <typename floatT>
    struct ControlSO2xR2
    {
        typename manif::SO2<floatT>::Tangent angular_velocity;
        Eigen::Matrix<floatT, 2, 1> euclidean_velocity;
    };
 
    template <typename floatT>
    struct SystemModelSO2xR2
    {
        static_assert(std::is_floating_point_v<floatT>);
 
        struct dim
        {
            static constexpr long state = 3, control = 3, obs = 3;
        };
 
        using FloatT = floatT;
        using StateT = StateSO2xR2<FloatT>;
        using ControlT = ControlSO2xR2<FloatT>;
        using ObsT = Eigen::Matrix<FloatT, dim::obs, 1>;
        using ControlNoiseT = Eigen::
            Matrix<FloatT, dim::control, 1>; // todo: correct name? Is it the same as ControlT?
        using SigmaPointsT = Eigen::Matrix<FloatT, dim::state, 1>; // todo: rename
 
        static ObsT observationFunction(const StateT& state);
 
        static StateT propagationFunction(const StateT& state,
                                          const ControlT& control,
                                          const ControlNoiseT& noise,
                                          FloatT dt);
 
        static StateT retraction(const StateT& state, const SigmaPointsT& sigmaPoints);
 
        static SigmaPointsT inverseRetraction(const StateT& state1, const StateT& state2);
    };
 
    extern template struct SystemModelSO2xR2<float>;
    extern template struct SystemModelSO2xR2<double>;
 
    //----------- SE2 -----------------
 
    template <typename floatT>
    struct StateSE2
    {
        manif::SE2<floatT> pose;
    };
 
    template <typename floatT>
    struct ControlSE2
    {
        typename manif::SE2<floatT>::Tangent velocity;
    };
 
    template <typename floatT>
    struct SystemModelSE2
    {
        static_assert(std::is_floating_point_v<floatT>);
 
        struct dim
        {
            static constexpr long state = 3, control = 3, obs = 3;
        };
 
        using FloatT = floatT;
        using StateT = StateSE2<FloatT>;
        using ControlT = ControlSE2<FloatT>;
        using ObsT = Eigen::Matrix<FloatT, dim::obs, 1>;
        using ControlNoiseT = Eigen::
            Matrix<FloatT, dim::control, 1>; // todo: correct name? Is it the same as ControlT?
        using SigmaPointsT = Eigen::Matrix<FloatT, dim::state, 1>; // todo: rename
 
        static ObsT observationFunction(const StateT& state);
 
        static StateT propagationFunction(const StateT& state,
                                          const ControlT& control,
                                          const ControlNoiseT& noise,
                                          FloatT dt);
 
        static StateT retraction(const StateT& state, const SigmaPointsT& sigmaPoints);
 
        static SigmaPointsT inverseRetraction(const StateT& state1, const StateT& state2);
    };
 
    extern template struct SystemModelSE2<float>;
    extern template struct SystemModelSE2<double>;
 
    //----------- SE2xV -----------------
 
    template <typename floatT>
    struct StateSE2xV
    {
        manif::SE2<floatT> pose;
        typename manif::SE2<floatT>::Tangent velocity;
    };
 
    template <typename floatT>
    struct ControlSE2xV
    {
    };
 
    template <typename floatT>
    struct SystemModelSE2xV
    {
        static_assert(std::is_floating_point_v<floatT>);
 
        struct dim
        {
            static constexpr long state = 6, control = 1, obs = 3;
        };
 
        using FloatT = floatT;
        using StateT = StateSE2xV<FloatT>;
        using ControlT = ControlSE2xV<FloatT>;
        using ObsT = Eigen::Matrix<FloatT, dim::obs, 1>;
        using ControlNoiseT = Eigen::
            Matrix<FloatT, dim::control, 1>; // todo: correct name? Is it the same as ControlT?
        using SigmaPointsT = Eigen::Matrix<FloatT, dim::state, 1>; // todo: rename
 
        static ObsT observationFunction(const StateT& state);
 
        static StateT propagationFunction(const StateT& state,
                                          const ControlT& control,
                                          const ControlNoiseT& noise,
                                          FloatT dt);
 
        static StateT retraction(const StateT& state, const SigmaPointsT& sigmaPoints);
 
        static SigmaPointsT inverseRetraction(const StateT& state1, const StateT& state2);
    };
 
    extern template struct SystemModelSE2xV<float>;
    extern template struct SystemModelSE2xV<double>;
 
} // namespace armarx::navigation::human::kalman_filter