including control input into measurement model?

[QingweiLau]

Dear @mherb, I am trying to use your helpful kalman filter library for humanoid motion state estimation, based on ukf.

And my state variable is center of mass position and velocity x(comx, comy, comz, dotcomx, dotcomy, dotcomz), the control input variable is zmp position and vertical force u(zmpx,zmpy,forcez), and measurement varible is com position from encoder, and acceleration from IMU z(comx_enc,comy_enc,comz_enc,acc_x,acc_y,acc_z).

And both the system model f(x,u)and measurement model h(x,u) are nonlinear, the definition can be found at the bottom. Then when compiling, I have an error when instantiating the MeasurementModel. And the error says

error: cannot declare variable ‘measurement_model’ to be of abstract type ‘UKF_LIPM::LIPM::LipMeasurementModel<float>’
     MeasurementModel measurement_model;
                      ^~~~~~~~~~~~~~~~~
In file included from /home/qingwei/catkin_ws/src/walk_stablization_controller/src/walk_stablization_controller.cpp:36:0:
/home/qingwei/catkin_ws/src/walk_stablization_controller/src/LipMeasurementModel.hpp:55:7: note:   because the following virtual functions are pure within ‘UKF_LIPM::LIPM::LipMeasurementModel<float>’:
 class LipMeasurementModel : public Kalman::LinearizedMeasurementModel<State<T>, LipMeasurement<T>, CovarianceBase>
       ^~~~~~~~~~~~~~~~~~~
In file included from /home/qingwei/catkin_ws/src/walk_stablization_controller/include/walk_stablization_controller/kalman/LinearizedMeasurementModel.hpp:25:0,
                 from /home/qingwei/catkin_ws/src/walk_stablization_controller/src/LipMeasurementModel.hpp:4,
                 from /home/qingwei/catkin_ws/src/walk_stablization_controller/src/walk_stablization_controller.cpp:36:
/home/qingwei/catkin_ws/src/walk_stablization_controller/include/walk_stablization_controller/kalman/MeasurementModel.hpp:60:29: note:      Kalman::MeasurementModel<StateType, MeasurementType, CovarianceBase>::Measurement Kalman::MeasurementModel<StateType, MeasurementType, CovarianceBase>::h(const State&) const [with StateType = UKF_LIPM::LIPM::State<float>; MeasurementType = UKF_LIPM::LIPM::LipMeasurement<float>; CovarianceBase = Kalman::StandardBase; Kalman::MeasurementModel<StateType, MeasurementType, CovarianceBase>::Measurement = UKF_LIPM::LIPM::LipMeasurement<float>; Kalman::MeasurementModel<StateType, MeasurementType, CovarianceBase>::State = UKF_LIPM::LIPM::State<float>]
         virtual Measurement h(const State& x) const = 0;

And I noticed in MeasurementModel.hpp the measurement function his only a function of state x, and no control input 'u'. Do you have any suggestions on where my problem is and how can I solve it? https://github.com/mherb/kalman/blob/9f40c2f7bedb08e964297a00a2b9360036590896/include/kalman/MeasurementModel.hpp#L59

I have defined the measurement model as

namespace UKF_LIPM
{
namespace LIPM
{
...
template<typename T, template<class> class CovarianceBase = Kalman::StandardBase>
class LipMeasurementModel : public Kalman::LinearizedMeasurementModel<State<T>, LipMeasurement<T>, CovarianceBase>
{
public:
    typedef UKF_LIPM::LIPM::State<T> S;
    typedef  UKF_LIPM::LIPM::LipMeasurement<T> M;
    typedef UKF_LIPM::LIPM::Control<T> C;
...    
    M h(const S& x, const C& u) const
    {
        M measurement;        
        measurement.cmx_enc() = x.cmx();
        measurement.cmy_enc() = x.cmy();
        measurement.cmz_enc() = x.cmz();
        measurement.accx_mea() = (x.cmx()-u.zmpx())/100.00/x.cmz()*u.forcez();
        measurement.accy_mea() = (x.cmy()-u.zmpy())/100.00/x.cmz()*u.forcez();
        measurement.accz_mea() = u.forcez()/100.00-9.8;

        return measurement;
    }
};

} // namespace LIPM
} // namespace UKF_LIPM

#endif

I attached the implementation and system/measurement model below, for your review.

Thank you in advance for your help.

Best regards. Qingwei

Implementation

using namespace UKF_LIPM;
typedef float T;

typedef LIPM::State<T> State;
typedef LIPM::Control<T> Control;
typedef LIPM::SystemModel<T> SystemModel;

typedef LIPM::LipMeasurement<T> Measurement;
typedef LIPM::LipMeasurementModel<T> MeasurementModel;

State ukf_x;
ukf_x.setZero();
Control ukf_u;
SystemModel ukf_sys;
 MeasurementModel measurement_model;
Kalman::UnscentedKalmanFilter<State> lipm_ukf(1,2,0);
lipm_ukf.init(ukf_x);   

System model LIPMSystemModel.hpp

#ifndef UKF_LIPM_SYSTEMMODEL_HPP_
#define UKF_LIPM_SYSTEMMODEL_HPP_

#include <kalman/LinearizedSystemModel.hpp>

namespace UKF_LIPM
{
namespace LIPM
{

/**
 * @brief 
 *
 * @param T Numeric scalar type
 */
template<typename T>
class State : public Kalman::Vector<T, 6>
{
public:
    KALMAN_VECTOR(State, T, 6)

    static constexpr size_t CMX = 0;
    static constexpr size_t CMY = 0;
    static constexpr size_t CMZ = 0.81;
    static constexpr size_t DOTCMX = 0;
    static constexpr size_t DOTCMY = 0;
    static constexpr size_t DOTCMZ = 0;

    T cmx()       const { return (*this)[ CMX ]; }
    T cmy()       const { return (*this)[ CMY ]; }
    T cmz()       const { return (*this)[ CMZ ]; }
    T dotcmx()    const { return (*this)[ DOTCMX ]; }
    T dotcmy()    const { return (*this)[ DOTCMY ]; }
    T dotcmz()    const { return (*this)[ DOTCMZ ]; }

    T& cmx()        { return (*this)[ CMX ]; }
    T& cmy()        { return (*this)[ CMY ]; }
    T& cmz()        { return (*this)[ CMZ ]; }
    T& dotcmx()     { return (*this)[ DOTCMX ]; }
    T& dotcmy()     { return (*this)[ DOTCMY ]; }
    T& dotcmz()     { return (*this)[ DOTCMZ ]; }
};

/**
 * @brief System control-input vector-type 
 *
 * @param T Numeric scalar type
 */
template<typename T>
class Control : public Kalman::Vector<T, 3>
{
public:
    KALMAN_VECTOR(Control, T, 3)

    static constexpr size_t ZMPX = 0;
    static constexpr size_t ZMPY = 0;
    static constexpr size_t FORCEZ = 0;

    T zmpx()       const { return (*this)[ ZMPX ]; }
    T zmpy()       const { return (*this)[ ZMPY ]; }
    T forcez()     const { return (*this)[ FORCEZ ]; }

    T& zmpx()        { return (*this)[ ZMPX ]; }
    T& zmpy()        { return (*this)[ ZMPY ]; }
    T& forcez()      { return (*this)[ FORCEZ ]; }

};

/**
 * @brief System model
 *
 * @param T Numeric scalar type
 * @param CovarianceBase Class template to determine the covariance representation
 *                       (as covariance matrix (StandardBase) or as lower-triangular
 *                       coveriace square root (SquareRootBase))
 */
template<typename T, template<class> class CovarianceBase = Kalman::StandardBase>
class SystemModel : public Kalman::LinearizedSystemModel<State<T>, Control<T>, CovarianceBase>
{
public:
    //! State type shortcut definition
    typedef UKF_LIPM::LIPM::State<T> S;

    //! Control type shortcut definition
    typedef UKF_LIPM::LIPM::Control<T> C;

    /**
     * @brief Definition of (non-linear) state transition function
     *  
     *
     * @param [in] x The system state in current time-step
     * @param [in] u The control vector input
     * @returns The (predicted) system state in the next time-step
     */
    S f(const S& x, const C& u) const
    {
        //! Predicted state vector after transition
        S x_;

        x_.cmx()=x.cmx()+x.dotcmx()*0.001;
        x_.cmy()=x.cmy()+x.dotcmy()*0.001;
        x_.cmz()=x.cmz()+x.dotcmz()*0.001;
        x_.dotcmx()=x.cmx()+((x.cmx()-u.zmpx())/100/x_.cmz()*u.forcez())*0.001;
        x_.dotcmy()=x.cmy()+((x.cmy()-u.zmpy())/100/x_.cmz()*u.forcez())*0.001;
        x_.dotcmx()=x.cmz()+(0.01*u.forcez()-9.8)*0.001;

        // Return transitioned state vector
        return x_;
    }    

};

} // namespace LIPM
} // namespace UKF_LIPM

#endif

Measurement model LipMeasurementModel.hpp

#ifndef UKF_LIPM_MEASUREMENTMODEL_HPP_
#define UKF_LIPM_MEASUREMENTMODEL_HPP_

#include <kalman/LinearizedMeasurementModel.hpp>

namespace UKF_LIPM
{
namespace LIPM
{

/**
 * @brief Measurement vector 
 *
 * @param T Numeric scalar type
 */
template<typename T>
class LipMeasurement : public Kalman::Vector<T, 6>
{
public:
    KALMAN_VECTOR(LipMeasurement, T, 6)

    //! Orientation
    static constexpr size_t CMX_ENC = 0;
    static constexpr size_t CMY_ENC = 0;
    static constexpr size_t CMZ_ENC = 0;
    static constexpr size_t ACCX_MEA = 0;
    static constexpr size_t ACCY_MEA = 0;
    static constexpr size_t ACCZ_MEA = 0;

    T cmx_enc()  const { return (*this)[ CMX_ENC ]; }
    T cmy_enc()  const { return (*this)[ CMY_ENC ]; }
    T cmz_enc()  const { return (*this)[ CMZ_ENC ]; }
    T accx_mea()  const { return (*this)[ ACCX_MEA ]; }
    T accy_mea()  const { return (*this)[ ACCY_MEA ]; }
    T accz_mea()  const { return (*this)[ ACCZ_MEA ]; }

    T& cmx_enc() { return (*this)[ CMX_ENC ]; }
    T& cmy_enc() { return (*this)[ CMY_ENC ]; }
    T& cmz_enc() { return (*this)[ CMZ_ENC ]; }
    T& accx_mea() { return (*this)[ ACCX_MEA ]; }
    T& accy_mea() { return (*this)[ ACCY_MEA ]; }
    T& accz_mea() { return (*this)[ ACCZ_MEA ]; }

};

/**
 * @brief Measurement model 
 *
 * @param T Numeric scalar type
 * @param CovarianceBase Class template to determine the covariance representation
 *                       (as covariance matrix (StandardBase) or as lower-triangular
 *                       coveriace square root (SquareRootBase))
 */
template<typename T, template<class> class CovarianceBase = Kalman::StandardBase>
class LipMeasurementModel : public Kalman::LinearizedMeasurementModel<State<T>, LipMeasurement<T>, CovarianceBase>
{
public:
    //! State type shortcut definition
    typedef UKF_LIPM::LIPM::State<T> S;

    //! Measurement type shortcut definition
    typedef  UKF_LIPM::LIPM::LipMeasurement<T> M;

    typedef UKF_LIPM::LIPM::Control<T> C;

    LipMeasurementModel()
    {
    //     
    }

    /**
     * @brief Definition of (possibly non-linear) measurement function

     *
     * @param [in] x The system state in current time-step
     * @returns The (predicted) sensor measurement for the system state
     */
    M h(const S& x, const C& u) const
    {
        M measurement;

        measurement.cmx_enc() = x.cmx();
        measurement.cmy_enc() = x.cmy();
        measurement.cmz_enc() = x.cmz();
        measurement.accx_mea() = (x.cmx()-u.zmpx())/100.00/x.cmz()*u.forcez();
        measurement.accy_mea() = (x.cmy()-u.zmpy())/100.00/x.cmz()*u.forcez();
        measurement.accz_mea() = u.forcez()/100.00-9.8;

        return measurement;
    }
};

} // namespace LIPM
} // namespace UKF_LIPM

#endif

[slchen]

The base class virtual interface of h(x) is virtual Measurement h(const State& x) const, but the h(x) in your derived class is M h(const S& x, const C& u) const.