// ============================================================================
int main(int argc,char **argv){
CarSimulator sim;
arr u = {.1, .2}; // fixed control signal
arr z;
ExtendedKalmanFilter ekf = ExtendedKalmanFilter(sim);
ekf.mu = {0., 0., 0.};
ekf.Sigma = eye(3);
// TODO set the matrices R, Q, and I of the ekf
// sim.observationNoise (use when evaluating a particle likelihood)
// sim.dynamicsNoise (use when propagating a particle)
sim.gl->watch();
for (uint t=0;t<1000;t++) {
sim.step(u);
sim.getRealNoisyObservation(z);
// EKF
ekf.predict(u);
ekf.correct(z);
// draw the belief
sim.gaussiansToDraw.resize(1);
sim.gaussiansToDraw(0).a = ekf.mu.sub(0, 1);
sim.gaussiansToDraw(0).A = ekf.Sigma.sub(0, 1, 0, 1);
arr tmp = ARR(sim.x, sim.y);
cout << "err trans: " << length(ekf.mu.sub(0, 1) - tmp) << endl;;
}
return 0;
}
// ============================================================================
int main(int argc,char **argv){
CarSimulator sim;
arr u = {.1, .2}; // fixed control signal
arr z;
ExtendedKalmanFilter ekf = ExtendedKalmanFilter(sim);
ekf.mu = {0., 0., 0.};
ekf.Sigma = eye(3);
ekf.I = eye(3);
ekf.R = sim.dynamicsNoise * eye(3,3);
ekf.Q = sim.observationNoise * eye(4,4);
std::cout << "R: " << ekf.R << std::endl;
std::cout << "Q: " << ekf.Q << std::endl;
// sim.observationNoise (use when evaluating a particle likelihood)
// sim.dynamicsNoise (use when propagating a particle)
sim.gl->watch();
for (uint t=0;t<1000;t++) {
sim.step(u);
sim.getRealNoisyObservation(z);
// EKF
ekf.predict(u);
ekf.correct(z);
// draw the belief
sim.gaussiansToDraw.resize(1);
sim.gaussiansToDraw(0).a = ekf.mu.sub(0, 1);
sim.gaussiansToDraw(0).A = ekf.Sigma.sub(0, 1, 0, 1);
arr tmp = ARR(sim.x, sim.y);
cout << "err trans: " << length(ekf.mu.sub(0, 1) - tmp) << endl;
// sleep for a few milliseconds or else the animation will be too fast to watch
usleep(7e3);
}
return 0;
}
void targetCallBack(const laser_package::state::ConstPtr& msg)
{
rho = msg->Measured_Rho;
theta = msg->Measured_Theta;
real_state(XI_INDEX) = msg->Real_X;
real_state(XI_DOT_INDEX) = msg->Real_X_Speed;
real_state(ETA_INDEX) = msg->Real_Y;
real_state(ETA_DOT_INDEX) = msg->Real_Y_Speed;
real_state(OMEGA_INDEX) = msg->Real_Omega;
z(RHO_INDEX) = rho;
z(THETA_INDEX) = theta;
if(msg_count>1)
{
update_time = msg->Time_Of_Measurement;
//extended kalman filter
ExtendedKF.updateFilter(z, update_time,real_state);
ExtendedKF.updateCovariance();
updateStateMessage(&ExtendedKF,msg);
extended_kalman_pub.publish(state_msg);
//regular kalman filter
KF.updateFilter(z, update_time, real_state);
KF.updateCovariance();
updateStateMessage(&KF,msg);
kalman_pub.publish(state_msg);
//imm
imm.update();
updateStateMessage(&imm,msg);
imm_pub.publish(state_msg);
}
else if(msg_count == 0)
{
first_xi = rho*cos(theta)+SENSOR_XI;
first_eta = rho*sin(theta)+SENSOR_ETA;
first_time = msg->Time_Of_Measurement;
msg_count++;
}
else if(msg_count==1)
{
second_time = msg->Time_Of_Measurement;
T = SAMPLING_INTERVAL;
initial_state(XI_INDEX) = rho*cos(theta)+SENSOR_XI;
initial_state(ETA_INDEX) = rho*sin(theta)+SENSOR_ETA;
initial_state(XI_DOT_INDEX) = (initial_state(XI_INDEX)-first_xi)/(T);
initial_state(ETA_DOT_INDEX) = (initial_state(ETA_INDEX)-first_eta)/(T);
initial_state(OMEGA_INDEX) = 0.0001;
//extended kalman filter
ExtendedKF = ExtendedKalmanFilter(initial_state,T , extended_kalman_noise_data,0.5,z);//instantiate Extended kalman filter
updateStateMessage(&ExtendedKF,msg);
extended_kalman_pub.publish(state_msg);
//regular kalman filter
KF = KalmanFilter(initial_state,T , kalman_noise_data, 0.5,z);//instantiate kalman filter
updateStateMessage(&KF,msg);
kalman_pub.publish(state_msg);
//imm
filters.push_back(&KF);
filters.push_back(&ExtendedKF);
imm = IMM(filters);//instantiate IMM with vector of filters
imm.update();
msg_count++;
}
//These are for when we want to publish to Rviz
target_point_msg.x = msg->Real_X;
target_point_msg.y = msg->Real_Y;
target_point_stamped_msg.point = target_point_msg;
target_point_stamped_msg.header.frame_id = "/my_frame";
//The below allows us to publish values so that Rviz can plot. You decide which points to plot from the target class.
target_real_pub.publish(target_point_stamped_msg);
}
