extended_kalman_filter.h

Go to the documentation of this file.

#ifndef EXTENDED_KALMAN_FILTER_H
#define EXTENDED_KALMAN_FILTER_H
 
#include "bitrl/bitrl_types.h"
#include "Eigen/Dense"
#include <boost/noncopyable.hpp>
#include <map>
#include <string>
#include <iostream>
 
namespace bitrl{
namespace estimation{
    
 
template<typename MotionModelTp, typename ObservationModelTp>
class ExtendedKalmanFilter: private boost::noncopyable
{
public:
 
    typedef MotionModelTp motion_model_type;
    typedef ObservationModelTp observation_model_type;
    typedef typename motion_model_type::input_type motion_model_input_type;
    typedef typename motion_model_type::matrix_type matrix_type;
    typedef typename motion_model_type::state_type state_type;
    typedef typename observation_model_type::input_type observation_model_input_type;
 
    ExtendedKalmanFilter();
 
    ExtendedKalmanFilter(motion_model_type& motion_model,
                         const observation_model_type& observation_model);
 
    ~ExtendedKalmanFilter();
 
    void estimate(const std::tuple<motion_model_input_type, 
                                   observation_model_input_type>& input );
 
    void predict(const motion_model_input_type& input);
 
    void update(const observation_model_input_type& z);
 
    void set_motion_model(motion_model_type& motion_model)
    {motion_model_ptr_ = &motion_model;}
 
    void set_observation_model(const observation_model_type& observation_model)
    {observation_model_ptr_ = &observation_model;}
 
    void set_matrix(const std::string& name, const matrix_type& mat);
 
    bool has_matrix(const std::string& name)const;
 
    const state_type& get_state()const{return motion_model_ptr_->get_state();}
 
    state_type& get_state(){return motion_model_ptr_->get_state();}
 
    real_t get(const std::string& name)const{return motion_model_ptr_->get_state_property(name);}
 
    const DynMat<real_t>& operator[](const std::string& name)const;
 
    DynMat<real_t>& operator[](const std::string& name);
    
    ExtendedKalmanFilter& with_matrix(const std::string& name, const matrix_type& mat);
           
protected:
 
    motion_model_type* motion_model_ptr_;
 
    const observation_model_type* observation_model_ptr_;
 
    std::map<std::string, matrix_type> matrices_;
 
};  
 
template<typename MotionModelTp, typename ObservationModelTp>
ExtendedKalmanFilter<MotionModelTp,ObservationModelTp>::ExtendedKalmanFilter()
    :
    motion_model_ptr_(nullptr),
    observation_model_ptr_(nullptr)
{}
 
template<typename MotionModelTp, typename ObservationModelTp>
ExtendedKalmanFilter<MotionModelTp,
                     ObservationModelTp>::ExtendedKalmanFilter(motion_model_type& motion_model,
                                                               const observation_model_type& observation_model)
    :
    motion_model_ptr_(&motion_model),
    observation_model_ptr_(&observation_model)
{}
 
template<typename MotionModelTp, typename ObservationModelTp>
ExtendedKalmanFilter<MotionModelTp,
                     ObservationModelTp>::~ExtendedKalmanFilter()
{}
 
template<typename MotionModelTp, typename ObservationModelTp>
void
ExtendedKalmanFilter<MotionModelTp,
                     ObservationModelTp>::set_matrix(const std::string& name,
                                                     const matrix_type& mat){
 
    if(name != "Q" && name != "K" && name != "R" && name != "P"){
        throw std::logic_error("Invalid matrix name. Name: "+
                               name+
                               " not in [Q, K, R, P]");
    }
 
    matrices_.insert_or_assign(name, mat);
}
 
template<typename MotionModelTp, typename ObservationModelTp>
ExtendedKalmanFilter<MotionModelTp, ObservationModelTp>& 
ExtendedKalmanFilter<MotionModelTp, ObservationModelTp>::with_matrix(const std::string& name, 
                                                                     const matrix_type& mat){
    set_matrix(name, mat);
    return *this;
}
 
template<typename MotionModelTp, typename ObservationModelTp>
bool
ExtendedKalmanFilter<MotionModelTp,ObservationModelTp>::has_matrix(const std::string& name)const{
 
    auto itr = matrices_.find(name);
    return itr != matrices_.end();
}
 
template<typename MotionModelTp, typename ObservationModelTp>
const DynMat<real_t>&
ExtendedKalmanFilter<MotionModelTp,ObservationModelTp>::operator[](const std::string& name)const{
 
    auto itr = matrices_.find(name);
 
    if(itr == matrices_.end()){
        throw std::invalid_argument("Matrix: "+name+" does not exist");
    }
 
    return itr->second;
}
 
template<typename MotionModelTp, typename ObservationModelTp>
DynMat<real_t>&
ExtendedKalmanFilter<MotionModelTp,ObservationModelTp>::operator[](const std::string& name){
 
    auto itr = matrices_.find(name);
 
    if(itr == matrices_.end()){
        throw std::invalid_argument("Matrix: "+name+" does not exist");
    }
 
    return itr->second;
}
 
 
template<typename MotionModelTp, typename ObservationModelTp>
void
ExtendedKalmanFilter<MotionModelTp,
                     ObservationModelTp>::estimate(const std::tuple<motion_model_input_type,
                                                   observation_model_input_type>& input ){
 
    predict(input.template get<0>());
    update(input.template get<1>());
}
 
template<typename MotionModelTp, typename ObservationModelTp>
void
ExtendedKalmanFilter<MotionModelTp,ObservationModelTp>::predict(const motion_model_input_type& u){
 
    motion_model_ptr_->evaluate(u);
 
    auto& P = (*this)["P"];
    auto& Q = (*this)["Q"];
 
    auto& L = motion_model_ptr_->get_matrix("L");
    auto L_T = L.transpose(); //trans(L);
 
    auto& F = motion_model_ptr_->get_matrix("F");
    auto F_T = F.transpose(); //trans(F);
 
    P = F * P * F_T + L*Q*L_T;
}
 
template<typename MotionModelTp, typename ObservationModelTp>
void
ExtendedKalmanFilter<MotionModelTp,
                     ObservationModelTp>::update(const observation_model_input_type&  z){
 
    auto& state = motion_model_ptr_->get_state();
    auto& P = (*this)["P"];
    auto& R = (*this)["R"];
 
    auto zpred = observation_model_ptr_->evaluate(z);
 
    auto& H = observation_model_ptr_->get_matrix("H");
    auto H_T = H.transpose(); //trans(H);
 
    // compute \partial{h}/\partial{v} the jacobian of the observation model
    // w.r.t the error vector
    auto& M = observation_model_ptr_->get_matrix("M");
    auto M_T = M.transpose(); //trans(M);
 
     try{
 
        // S = H*P*H^T + M*R*M^T
        auto S = H*P*H_T + M*R*M_T;
 
        auto S_inv = S.inverse(); //inv(S);
 
        if(has_matrix("K")){
            auto& K = (*this)["K"];
            K = P*H_T*S_inv;
        }
        else{
            auto K = P*H_T*S_inv;
            set_matrix("K", K);
        }
 
        auto& K = (*this)["K"];
 
        auto innovation = z - zpred;
        
        // we need to take the transpose
        auto  innovation_t = innovation.transpose();
 
        if(K.cols() != innovation_t.rows()){
            throw std::runtime_error("Matrix columns: "+
                                      std::to_string(K.cols())+
                                      " not equal to vector size: "+
                                      std::to_string(innovation_t.rows()));
        }
 
        //auto vec = K * innovation_t;
        state += K * innovation_t; //.add(K*innovation);
 
        //IdentityMatrix<real_t> I(state.size());
        auto I = matrix_type::Identity(state.size(), state.size());
        P =  (I - K*H)*P;
    }
    catch(...){
 
        // this is a singular matrix what
        // should we do? Simply use the predicted
        // values and log the fact that there was a singular matrix
 
        throw;
    }
}
 
    
}
}
 
 
#endif