kalman_filter.h

Go to the documentation of this file.

#include "bitrl/bitrl_types.h"
#include "bitrl/utils/std_map_utils.h"
#include "Eigen/Dense"
#include <boost/noncopyable.hpp>
 
#include <any>
#include <map>
#include <string>
#include <stdexcept> //for std::invalid_argument
 
 
namespace bitrl{
namespace estimation{
    
 
 
template<typename MotionModelType, typename ObservationModelType>
struct KalmanFilterConfig
{
    
    
    typedef MotionModelType motion_model_type;
    typedef ObservationModelType observation_model_type;
 
    motion_model_type* motion_model;
    
    const observation_model_type* observation_model;
 
    DynMat<real_t> B;
    DynMat<real_t> P;
    DynMat<real_t> Q;
    DynMat<real_t> K;
    DynMat<real_t> R;
};
 
template<typename MotionModelType, typename ObservationModelType>
class KalmanFilter: private boost::noncopyable
{
 
public:
    
    typedef KalmanFilterConfig<MotionModelType, ObservationModelType>  config_type;
    typedef DynMat<real_t> matrix_type;
    
    typedef MotionModelType motion_model_type;
    typedef ObservationModelType observation_model_type;
    typedef typename config_type::motion_model_type::state_type state_type;
    typedef std::map<std::string, std::any> input_type;
 
    KalmanFilter(const KalmanFilterConfig<MotionModelType, ObservationModelType>& config);
 
    state_type& estimate(const input_type& input );
 
    void predict(const input_type& input);
 
    void update(const input_type& input);
 
    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;}
    
    KalmanFilter& with_matrix(const std::string& name, const matrix_type& mat);
 
    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(name);}
 
    const matrix_type& operator[](const std::string& name)const;
 
    matrix_type& operator[](const std::string& name);
 
private:
 
    motion_model_type* motion_model_ptr_;
 
    const observation_model_type* observation_model_ptr_;
 
    std::map<std::string, matrix_type> matrices_;
};
 
 
template<typename MotionModelType, typename ObservationModelType>
KalmanFilter<MotionModelType,
             ObservationModelType>::KalmanFilter(const KalmanFilterConfig<MotionModelType, ObservationModelType>& config)
    :
    motion_model_ptr_(config.motion_model),
    observation_model_ptr_(config.observation_model)
{
    // set the matrices
    set_matrix("B", config.B);
    set_matrix("P", config.P);
    set_matrix("Q", config.Q);
    set_matrix("K", config.K);
    set_matrix("R", config.R);
}
 
 
template<typename MotionModelType, typename ObservationModelType>
const DynMat<real_t>&
KalmanFilter<MotionModelType,ObservationModelType>::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 MotionModelType, typename ObservationModelType>
DynMat<real_t>&
KalmanFilter<MotionModelType,ObservationModelType>::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 MotionModelType, typename ObservationModelType>
KalmanFilter<MotionModelType, ObservationModelType>&
KalmanFilter<MotionModelType, 
             ObservationModelType>::with_matrix(const std::string& name, const matrix_type& mat){
 
        set_matrix(name, mat);
        return *this;
}
 
template<typename MotionModelType, typename ObservationModelType>
void
KalmanFilter<MotionModelType, 
             ObservationModelType>::set_matrix(const std::string& name,
                                               const matrix_type& mat){
 
    if(name != "Q" && 
       name != "K" && 
       name != "R" && 
       name != "P" &&
       name != "B"){
        throw std::logic_error("Invalid matrix name. Name: "+
                               name+
                               " not in [Q, K, R, P, B]");
    }
 
    matrices_.insert_or_assign(name, mat);
}
 
template<typename MotionModelType, typename ObservationModelType>
bool
KalmanFilter<MotionModelType,ObservationModelType>::has_matrix(const std::string& name)const{
 
    auto itr = matrices_.find(name);
    return itr != matrices_.end();
}
 
template<typename MotionModelType, typename ObservationModelType>
typename KalmanFilter<MotionModelType, ObservationModelType>::state_type&
KalmanFilter<MotionModelType,
                     ObservationModelType>::estimate(const input_type& input ){
    predict(input);
    update(input);
    
    return get_state();
}
 
template<typename MotionModelType, typename ObservationModelType>
void
KalmanFilter<MotionModelType,
             ObservationModelType>::predict(const input_type& input ){
 
    if(!motion_model_ptr_){
        throw std::runtime_error("Motion model has not been set");
    }
 
    auto u = bitrl::utils::template resolve<DynVec<real_t>>("u", input);
    auto w = bitrl::utils::template resolve<DynVec<real_t>>("w", input);
 
    // make a state predicion using the
    // motion model
    auto& x = motion_model_ptr_->get_state();
    
    // get the matrix that describes the dynamics
    // of the system
    auto& F = motion_model_ptr_->get_matrix("F");
    auto& B = (*this)["B"];
 
    // update the state vector
    x = F*x + B*u + w;
    //state.set(x);
 
    // predict the covariance matrix
    auto& P = (*this)["P"];
    auto& Q = (*this)["Q"];
    auto F_T =  F.transpose();
 
    P = (F*P*F_T) + Q;
}
 
template<typename MotionModelType, typename ObservationModelType>
void
KalmanFilter<MotionModelType,
             ObservationModelType>::update(const input_type& input){
 
    if(!motion_model_ptr_){
        throw std::runtime_error("Motion model has not been set");
    }
 
    if(!observation_model_ptr_){
        throw std::runtime_error("Observation model has not been set");
    }
 
    auto& x = motion_model_ptr_->get_state();
    auto& P = (*this)["P"];
    auto& R = (*this)["R"];
 
    auto& H = observation_model_ptr_->get_matrix("H");
    auto H_T = H.transpose();
 
    try{
 
      auto S = H*P*H_T + R;
      auto S_inv = S.inverse(); //inv(S);
 
      // compute the gain matrix
      if(has_matrix("K")){
          auto& K = matrices_["K"];
          K = P*H_T*S_inv;
      }
      else{
          auto K = P*H_T*S_inv;
          set_matrix("K", K);
      }
 
      auto& K = (*this)["K"];
      auto z = bitrl::utils::template resolve<DynVec<real_t>>("z", input);
      auto innovation = z - H*x;
 
      if(K.cols() != innovation.size()){
          throw std::runtime_error("Matrix columns: "+
                                    std::to_string(K.cols())+
                                    " not equal to vector size: "+
                                    std::to_string(innovation.size()));
      }
 
      x += K*innovation;
      //state.set(x);
 
      auto I = matrix_type::Identity(x.size(), x.size());
      
      // update the error covariance matrix
      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;
    }
}
 
}
}