Environment.h

#ifndef ENVIRONMENT_H
#define ENVIRONMENT_H

#include <nodelet/nodelet.h>
#include <ros/ros.h>
#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/matrix.hpp>

#include "car/ussDataMsg.h"
#include "car/camDataMsg.h"

using namespace boost::numeric::ublas;
namespace car
{
class Environment : public nodelet::Nodelet
{
public:
    virtual void onInit();

    Environment(ros::NodeHandle & nh, std::string & name);

    Environment();

    ~Environment();

private:
    ros::NodeHandle nh_;
    std::string name_;

    float distance;
    float relativeSpeed;

    ros::Publisher environmentData;
    ros::Subscriber ussData;
    ros::Subscriber camData;


    void ussDataCallback(const ussDataMsg::ConstPtr & inMsg);

    void camDataCallback(const camDataMsg::ConstPtr & inMsg);

    zero_vector<double> x {2};
    zero_matrix<double> P {2, 2};
    zero_matrix<double> F {2, 2};
    zero_matrix<double> Q {2, 2};
    zero_vector<double> H {2};
    zero_vector<double> R {1};
    
    void predict();
    void update(vector<double> mesVec);
};
}
#endif