void imuCallback(const sensor_msgs::ImuConstPtr& msg) {
Eigen::Vector3d accel_measurement, gyro_measurement;
tf::vectorMsgToEigen(msg->angular_velocity, gyro_measurement);
tf::vectorMsgToEigen(msg->linear_acceleration, accel_measurement);
Matrix3 linear_acceleration_covariance(&msg->linear_acceleration_covariance[0]);
Matrix3 angular_velocity_covariance(&msg->angular_velocity_covariance[0]);
double msg_time ;
msg_time = msg->header.stamp.toSec();
std::cout<<"IMU Callback: Time:"<<msg_time<<std::endl;
double dt = msg_time - last_msg_time;
last_msg_time = msg_time;
//TODO set covariance
ukf->predict(accel_measurement,gyro_measurement,dt, linear_acceleration_covariance, angular_velocity_covariance);
std::cout<<"Acc Cov:"<<"\n"<<linear_acceleration_covariance<<"\n"<<"Vel Cov"<<"\n"<<angular_velocity_covariance<<std::endl;
sendControlSignal();
}
void connectionHandleRfcommControlReq(const CL_INTERNAL_RFCOMM_CONTROL_REQ_T* req)
{
sendControlSignal(req->sink, req->break_signal, req->modem_signal);
}
