// constructor
TrackerKalman::TrackerKalman(const string& name, const StatePosVel& sysnoise):
Tracker(name),
filter_(NULL),
sys_pdf_(NULL),
sys_model_(NULL),
meas_pdf_(NULL),
meas_model_(NULL),
sys_matrix_(6,6),
tracker_initialized_(false)
{
// create sys model
sys_matrix_ = 0;
for (unsigned int i=1; i<=3; i++){
sys_matrix_(i,i) = 1;
sys_matrix_(i+3,i+3) = damping_velocity;
}
ColumnVector sys_mu(6); sys_mu = 0;
sys_sigma_ = SymmetricMatrix(6); sys_sigma_ = 0;
for (unsigned int i=0; i<3; i++){
sys_sigma_(i+1, i+1) = pow(sysnoise.pos_[i],2);
sys_sigma_(i+4, i+4) = pow(sysnoise.vel_[i],2);
}
Gaussian sys_noise(sys_mu, sys_sigma_);
sys_pdf_ = new LinearAnalyticConditionalGaussian(sys_matrix_, sys_noise);
sys_model_ = new LinearAnalyticSystemModelGaussianUncertainty(sys_pdf_);
// create meas model
Matrix meas_matrix(3,6); meas_matrix = 0;
for (unsigned int i=1; i<=3; i++)
meas_matrix(i,i) = 1;
ColumnVector meas_mu(3); meas_mu = 0;
SymmetricMatrix meas_sigma(3); meas_sigma = 0;
for (unsigned int i=0; i<3; i++)
meas_sigma(i+1, i+1) = 0;
Gaussian meas_noise(meas_mu, meas_sigma);
meas_pdf_ = new LinearAnalyticConditionalGaussian(meas_matrix, meas_noise);
meas_model_ = new LinearAnalyticMeasurementModelGaussianUncertainty(meas_pdf_);
};
int main(int argc, char **argv)
{
//ROS stuff
ros::init(argc, argv, "simulate_node");
ros::NodeHandle n;
ros::Publisher filter_target_pub = n.advertise<laser_package::state>("/filter_topic",1000); //publish targets to new topic
//ros::ServiceClient client_update = n.serviceClient<laser_package::update_tracker>("updateFilter");
//ros::ServiceClient client_initialize = n.serviceClient<laser_package::update_tracker>("initializeFilter");
laser_package::update_tracker srv;
laser_package::state state_msg;
ros::Rate r(floor(1/SAMPLING_INTERVAL+0.5));
//random noise stuff
std::default_random_engine measurement_generator;
std::normal_distribution<double> meas_noise(MU_W_SIMULATE,VAR_W_SIMULATE);
//simulator class
Simulator simulator = Simulator();
//other variables
Eigen::VectorXd next_x(5), past_x(5);
double w;
past_x << 0, 10, 0, 0, OMEGA_RADS; // initial vector: xi, xi_dot, eta, eta_dot, omega
simulator.initializeSimulators(past_x);
/*srv.request.initial_x = past_x(XI);
srv.request.initial_x_velocity = past_x(XI_DOT);
srv.request.initial_y = past_x(ETA);
srv.request.initial_y_velocity = past_x(ETA_DOT);
srv.request.initial_turn_rate = past_x(OMEGA);
srv.request.sampling_interval = SAMPLING_INTERVAL;
srv.request.update_time = ros::Time::now().toSec();
srv.request.measurement_noise_mean = MU_W;
srv.request.measurement_noise_variance = VAR_W;*/
//initialize tracker(s)
int counter = 0;
//client_initialize.call(srv);
while(ros::ok())
{
w = meas_noise(measurement_generator);//noises
if(counter<300)
{
next_x = simulator.simulateCoordinatedTurn(OMEGA_RADS);
//next_x = simulator.simulateUniformMotion();
}
else if (counter>300&counter<500)
{
//next_x = simulator.simulateUniformMotion();
next_x = simulator.simulateCoordinatedTurn(OMEGA_RADS);
}
else if (counter>500&&counter<1100)
{
next_x = simulator.simulateUniformMotion();
//next_x = simulator.simulateCoordinatedTurn(OMEGA_RADS);
}
else
{
next_x = simulator.simulateUniformMotion();
//next_x = simulator.simulateCoordinatedTurn(OMEGA_RADS);
}
//update values
state_msg.Real_X = next_x(XI);
state_msg.Measured_X = next_x(XI) + w;
state_msg.Real_Y = next_x(ETA);
state_msg.Measured_Y = next_x(ETA) + w;
state_msg.Time_Of_Measurement = ros::Time::now().toSec();
filter_target_pub.publish(state_msg);
counter++;
//send update
//if(client_update.call(srv));
ros::spinOnce();
r.sleep();
}
return 0;
}
