HumanSystemModel.cpp

Go to the documentation of this file.

#include "HumanSystemModel.h"
 
namespace armarx::navigation::human::kalman_filter
{
 
    // ------------------------------ SO2xR2 -----------------------------------------
 
    template <typename floatT>
    typename SystemModelSO2xR2<floatT>::ObsT
    SystemModelSO2xR2<floatT>::observationFunction(const SystemModelSO2xR2::StateT& state)
    {
        ObsT observation;
        observation.segment(0, 1) = state.orientation.log().coeffs();
        observation.segment(1, 2) = state.position;
        return observation;
    }
 
    template <typename floatT>
    typename SystemModelSO2xR2<floatT>::StateT
    SystemModelSO2xR2<floatT>::propagationFunction(const SystemModelSO2xR2::StateT& state,
                                                   const SystemModelSO2xR2::ControlT& control,
                                                   const SystemModelSO2xR2::ControlNoiseT& noise,
                                                   FloatT dt)
    {
        StateT new_state;
        new_state.orientation = state.orientation.template rplus(control.angular_velocity * dt);
        new_state.position = state.position + control.euclidean_velocity * dt;
        //new_state.position =
        //   state.position + state.orientation.rotation() * control.euclidean_velocity * dt;
        return new_state;
    }
 
    template <typename floatT>
    typename SystemModelSO2xR2<floatT>::StateT
    SystemModelSO2xR2<floatT>::retraction(const SystemModelSO2xR2::StateT& state,
                                          const SystemModelSO2xR2::SigmaPointsT& sigmaPoints)
    {
        StateT new_state;
        manif::SO2Tangent<FloatT> tan;
        tan.coeffs() << sigmaPoints.segment(0, 1);
        //new_state.orientation = state.orientation.lplus(tan);
        new_state.orientation = state.orientation.rplus(tan);
 
        new_state.position = state.position + sigmaPoints.segment(1, 2);
        return new_state;
    }
 
    template <typename floatT>
    typename SystemModelSO2xR2<floatT>::SigmaPointsT
    SystemModelSO2xR2<floatT>::inverseRetraction(const SystemModelSO2xR2::StateT& state1,
                                                 const SystemModelSO2xR2::StateT& state2)
    {
        SigmaPointsT sigma;
        // TODO: check if right order of substraction
 
        //sigma.segment(0, 1) = state2.orientation.lminus(state1.orientation).coeffs();
        sigma.segment(0, 1) = state1.orientation.lminus(state2.orientation).coeffs();
 
        sigma.segment(1, 2) = state1.position - state2.position;
        //sigma.segment(1, 2) = state2.position - state1.position;
 
        return sigma;
    }
 
    template struct SystemModelSO2xR2<float>;
    template struct SystemModelSO2xR2<double>;
 
    // ------------------------------ SE2 -----------------------------------------
 
    template <typename floatT>
    typename SystemModelSE2<floatT>::ObsT
    SystemModelSE2<floatT>::observationFunction(const SystemModelSE2::StateT& state)
    {
        ObsT observation = state.pose.log().coeffs();
        return observation;
    }
 
    template <typename floatT>
    typename SystemModelSE2<floatT>::StateT
    SystemModelSE2<floatT>::propagationFunction(const SystemModelSE2::StateT& state,
                                                const SystemModelSE2::ControlT& control,
                                                const SystemModelSE2::ControlNoiseT& noise,
                                                FloatT dt)
    {
        StateT new_state;
        new_state.pose = state.pose.template rplus(control.velocity * dt);
        return new_state;
    }
 
    template <typename floatT>
    typename SystemModelSE2<floatT>::StateT
    SystemModelSE2<floatT>::retraction(const SystemModelSE2::StateT& state,
                                       const SystemModelSE2::SigmaPointsT& sigmaPoints)
    {
        StateT new_state;
        manif::SE2Tangent<FloatT> tan;
        tan.coeffs() << sigmaPoints;
        new_state.pose = state.pose.lplus(tan);
        return new_state;
    }
 
    template <typename floatT>
    typename SystemModelSE2<floatT>::SigmaPointsT
    SystemModelSE2<floatT>::inverseRetraction(const SystemModelSE2::StateT& state1,
                                              const SystemModelSE2::StateT& state2)
    {
        SigmaPointsT sigma;
        sigma = state2.pose.lminus(state1.pose).coeffs();
        return sigma;
    }
 
    template struct SystemModelSE2<float>;
    template struct SystemModelSE2<double>;
 
    // ------------------------------ SE2xV -----------------------------------------
 
    template <typename floatT>
    typename SystemModelSE2xV<floatT>::ObsT
    SystemModelSE2xV<floatT>::observationFunction(const SystemModelSE2xV::StateT& state)
    {
        ObsT observation = state.pose.log().coeffs();
        return observation;
    }
 
    template <typename floatT>
    typename SystemModelSE2xV<floatT>::StateT
    SystemModelSE2xV<floatT>::propagationFunction(const SystemModelSE2xV::StateT& state,
                                                  const SystemModelSE2xV::ControlT& control,
                                                  const SystemModelSE2xV::ControlNoiseT& noise,
                                                  FloatT dt)
    {
        StateT new_state;
        new_state.pose = state.pose.template rplus(state.velocity * dt);
        new_state.velocity = state.velocity;
        return new_state;
    }
 
    template <typename floatT>
    typename SystemModelSE2xV<floatT>::StateT
    SystemModelSE2xV<floatT>::retraction(const SystemModelSE2xV::StateT& state,
                                         const SystemModelSE2xV::SigmaPointsT& sigmaPoints)
    {
        StateT new_state;
        manif::SE2Tangent<FloatT> tan;
        tan.coeffs() << sigmaPoints.segment(0, 3);
        new_state.pose = state.pose.lplus(tan);
        tan.coeffs() << sigmaPoints.segment(3, 3);
        // TODO: this is probably not correct, i.e. there needs to be some interaction between velocity and pose
        new_state.velocity += tan;
        return new_state;
    }
 
    template <typename floatT>
    typename SystemModelSE2xV<floatT>::SigmaPointsT
    SystemModelSE2xV<floatT>::inverseRetraction(const SystemModelSE2xV::StateT& state1,
                                                const SystemModelSE2xV::StateT& state2)
    {
        SigmaPointsT sigma;
        sigma.template segment<3>(0) = state2.pose.lminus(state1.pose).coeffs();
        // TODO: this is probably not correct; probably one cannot subtract two tangent vectors at two different poses
        //  from each other
        sigma.template segment<3>(3) = (state2.velocity - state1.velocity).coeffs();
        return sigma;
    }
 
    template struct SystemModelSE2xV<float>;
    template struct SystemModelSE2xV<double>;
 
} // namespace armarx::navigation::human::kalman_filter