void RTFusionKalman4::reset()
{
m_firstTime = true;
m_fusionPose = RTVector3();
m_fusionQPose.fromEuler(m_fusionPose);
m_gyro = RTVector3();
m_accel = RTVector3();
m_compass = RTVector3();
m_measuredPose = RTVector3();
m_measuredQPose.fromEuler(m_measuredPose);
m_Rk.fill(0);
m_Q.fill(0);
// initialize process noise covariance matrix
for (int i = 0; i < KALMAN_STATE_LENGTH; i++)
for (int j = 0; j < KALMAN_STATE_LENGTH; j++)
m_Q.setVal(i, i, KALMAN_QVALUE);
// initialize observation noise covariance matrix
for (int i = 0; i < KALMAN_STATE_LENGTH; i++)
for (int j = 0; j < KALMAN_STATE_LENGTH; j++)
m_Rk.setVal(i, i, KALMAN_RVALUE);
}
void RTFusionRTQF::reset()
{
m_firstTime = true;
m_fusionPose = RTVector3();
m_fusionQPose.fromEuler(m_fusionPose);
m_measuredPose = RTVector3();
m_measuredQPose.fromEuler(m_measuredPose);
}
void RTFusionAHRS::reset()
{
m_firstTime = true;
m_fusionPose = RTVector3();
m_fusionQPose.fromEuler(m_fusionPose);
m_gyro = RTVector3();
m_accel = RTVector3();
m_compass = RTVector3();
m_measuredPose = RTVector3();
m_measuredQPose.fromEuler(m_measuredPose);
}
void RTFusionRTQF::reset()
{
m_firstTime = true;
m_fusionPose = RTVector3();
m_fusionQPose.fromEuler(m_fusionPose);
m_gyro = RTVector3();
m_accel = RTVector3();
m_compass = RTVector3();
m_measuredPose = RTVector3();
m_measuredQPose.fromEuler(m_measuredPose);
m_sampleNumber = 0;
}
CompassCalDlg::CompassCalDlg(QWidget *parent)
: QDialog(parent, Qt::WindowCloseButtonHint | Qt::WindowTitleHint)
{
layoutWindow();
setWindowTitle("Compass calibration");
connect(m_buttons, SIGNAL(accepted()), this, SLOT(onOk()));
connect(m_buttons, SIGNAL(rejected()), this, SLOT(onCancel()));
m_timer = startTimer(100);
m_compassMin = RTVector3(10000, 10000, 10000);
m_compassMax = RTVector3(-10000, -10000, -10000);
}
void RTIMUMagCal::magCalReset()
{
m_magMin = RTVector3(RTIMUCALDEFS_DEFAULT_MIN, RTIMUCALDEFS_DEFAULT_MIN, RTIMUCALDEFS_DEFAULT_MIN);
m_magMax = RTVector3(RTIMUCALDEFS_DEFAULT_MAX, RTIMUCALDEFS_DEFAULT_MAX, RTIMUCALDEFS_DEFAULT_MAX);
m_magCalCount = 0;
for (int i = 0; i < RTIMUCALDEFS_OCTANT_COUNT; i++)
m_octantCounts[i] = 0;
m_magCalInIndex = m_magCalOutIndex = 0;
// throw away first few samples so we don't see any old calibrated samples
m_startCount = 100;
}
bool RTIMUMagCal::magCalSaveCorr(const char *ellipsoidFitPath)
{
FILE *file;
char *corrFile;
float a[3];
float b[9];
if (ellipsoidFitPath != NULL) {
corrFile = (char *)malloc(strlen(RTIMUCALDEFS_MAG_CORR_FILE) + strlen(ellipsoidFitPath) + 2);
sprintf(corrFile, "%s/%s", ellipsoidFitPath, RTIMUCALDEFS_MAG_CORR_FILE);
if ((file = fopen(corrFile, "r")) == NULL) {
HAL_ERROR("Failed to open ellipsoid fit correction data file\n");
return false;
}
if (fscanf(file, "%f %f %f %f %f %f %f %f %f %f %f %f",
a + 0, a + 1, a + 2, b + 0, b + 1, b + 2, b + 3, b + 4, b + 5, b + 6, b + 7, b + 8) != 12) {
HAL_ERROR("Ellipsoid corrcetion file didn't have 12 floats\n");
fclose(file);
return false;
}
fclose(file);
m_settings->m_compassCalEllipsoidValid = true;
m_settings->m_compassCalEllipsoidOffset = RTVector3(a[0], a[1], a[2]);
memcpy(m_settings->m_compassCalEllipsoidCorr, b, 9 * sizeof(float));
m_settings->saveSettings();
return true;
}
return false;
}
void RTFusionRTQF::newIMUData(RTIMU_DATA& data, const RTIMUSettings *settings)
{
if (m_debug) {
HAL_INFO("\n------\n");
HAL_INFO2("IMU update delta time: %f, sample %d\n", m_timeDelta, m_sampleNumber++);
}
m_sampleNumber++;
if (m_enableGyro)
m_gyro = data.gyro;
else
m_gyro = RTVector3();
m_accel = data.accel;
m_compass = data.compass;
m_compassValid = data.compassValid;
if (m_firstTime) {
m_lastFusionTime = data.timestamp;
calculatePose(m_accel, m_compass, settings->m_compassAdjDeclination);
m_Fk.fill(0);
// initialize the poses
m_stateQ.fromEuler(m_measuredPose);
m_fusionQPose = m_stateQ;
m_fusionPose = m_measuredPose;
m_firstTime = false;
} else {
m_timeDelta = (RTFLOAT)(data.timestamp - m_lastFusionTime) / (RTFLOAT)1000000;
m_lastFusionTime = data.timestamp;
if (m_timeDelta <= 0)
return;
calculatePose(data.accel, data.compass, settings->m_compassAdjDeclination);
predict();
update();
m_stateQ.toEuler(m_fusionPose);
m_fusionQPose = m_stateQ;
if (m_debug) {
HAL_INFO(RTMath::displayRadians("Measured pose", m_measuredPose));
HAL_INFO(RTMath::displayRadians("RTQF pose", m_fusionPose));
HAL_INFO(RTMath::displayRadians("Measured quat", m_measuredPose));
HAL_INFO(RTMath::display("RTQF quat", m_stateQ));
HAL_INFO(RTMath::display("Error quat", m_stateQError));
}
}
data.fusionPoseValid = true;
data.fusionQPoseValid = true;
data.fusionPose = m_fusionPose;
data.fusionQPose = m_fusionQPose;
}
void RTFusionKalman4::newIMUData(RTIMU_DATA& data, const RTIMUSettings *settings)
{
if (m_enableGyro)
m_gyro = data.gyro;
else
m_gyro = RTVector3();
m_accel = data.accel;
m_compass = data.compass;
m_compassValid = data.compassValid;
if (m_firstTime) {
m_lastFusionTime = data.timestamp;
calculatePose(m_accel, m_compass, settings->m_compassAdjDeclination);
m_Fk.fill(0);
// init covariance matrix to something
m_Pkk.fill(0);
for (int i = 0; i < 4; i++)
for (int j = 0; j < 4; j++)
m_Pkk.setVal(i,j, 0.5);
// initialize the observation model Hk
// Note: since the model is the state vector, this is an identity matrix so it won't be used
// initialize the poses
m_stateQ.fromEuler(m_measuredPose);
m_fusionQPose = m_stateQ;
m_fusionPose = m_measuredPose;
m_firstTime = false;
} else {
m_timeDelta = (RTFLOAT)(data.timestamp - m_lastFusionTime) / (RTFLOAT)1000000;
m_lastFusionTime = data.timestamp;
if (m_timeDelta <= 0)
return;
if (m_debug) {
HAL_INFO("\n------\n");
HAL_INFO1("IMU update delta time: %f\n", m_timeDelta);
}
calculatePose(data.accel, data.compass, settings->m_compassAdjDeclination);
predict();
update();
m_stateQ.toEuler(m_fusionPose);
m_fusionQPose = m_stateQ;
if (m_debug | settings->m_fusionDebug) {
HAL_INFO(RTMath::displayRadians("Measured pose", m_measuredPose));
HAL_INFO(RTMath::displayRadians("Kalman pose", m_fusionPose));
HAL_INFO(RTMath::displayRadians("Measured quat", m_measuredPose));
HAL_INFO(RTMath::display("Kalman quat", m_stateQ));
HAL_INFO(RTMath::display("Error quat", m_stateQError));
}
}
data.fusionPoseValid = true;
data.fusionQPoseValid = true;
data.fusionPose = m_fusionPose;
data.fusionQPose = m_fusionQPose;
}
int main()
{
int sampleCount = 0;
int sampleRate = 0;
uint64_t rateTimer;
uint64_t displayTimer;
uint64_t now;
// using RTIMULib here allows it to use the .ini file generated by RTIMULibDemo.
RTIMUSettings *settings = new RTIMUSettings("RTIMULib");
RTIMU *imu = RTIMU::createIMU(settings);
if ((imu == NULL) || (imu->IMUType() == RTIMU_TYPE_NULL)) {
printf("No IMU found\n");
exit(1);
}
// This is an opportunity to manually override any settings before the call IMUInit
// set up IMU
imu->IMUInit();
// set up for rate timer
rateTimer = displayTimer = RTMath::currentUSecsSinceEpoch();
// create the vrpn servee
vrpn = new vrpnServer();
// now just process data
while (1) {
// poll at the rate recommended by the IMU
usleep(imu->IMUGetPollInterval() * 1000);
// call the vrpn main loop
vrpn->mainloop(); // this function can be placed inside a new thread, which should run faster than
// the loop that calls the update_tracking function.
while (imu->IMURead()) {
RTIMU_DATA imuData = imu->getIMUData();
vrpn->serverTracker->update_tracking(RTVector3(), imuData.fusionQPose);
sampleCount++;
now = RTMath::currentUSecsSinceEpoch();
// display 10 times per second
if ((now - displayTimer) > 100000) {
printf("Sample rate %d: %s\r", sampleRate, RTMath::displayDegrees("", imuData.fusionPose));
fflush(stdout);
displayTimer = now;
}
// update rate every second
if ((now - rateTimer) > 1000000) {
sampleRate = sampleCount;
sampleCount = 0;
rateTimer = now;
}
}
}
}
void RTFusionRTQF::newIMUData(const RTVector3& gyro, const RTVector3& accel,
const RTVector3& compass, unsigned long timestamp)
{
RTVector3 fusionGyro;
if (m_firstTime) {
m_lastFusionTime = timestamp;
calculatePose(accel, compass);
// initialize the poses
m_fusionQPose.fromEuler(m_measuredPose);
m_fusionPose = m_measuredPose;
m_firstTime = false;
} else {
m_timeDelta = (RTFLOAT)(timestamp - m_lastFusionTime) / (RTFLOAT)1000;
m_lastFusionTime = timestamp;
if (m_timeDelta <= 0) {
return;
}
calculatePose(accel, compass);
// predict();
RTFLOAT x2, y2, z2;
RTFLOAT qs, qx, qy, qz;
qs = m_fusionQPose.scalar();
qx = m_fusionQPose.x();
qy = m_fusionQPose.y();
qz = m_fusionQPose.z();
if (m_enableGyro) {
fusionGyro = gyro;
} else {
fusionGyro = RTVector3();
}
x2 = fusionGyro.x() / (RTFLOAT)2.0;
y2 = fusionGyro.y() / (RTFLOAT)2.0;
z2 = fusionGyro.z() / (RTFLOAT)2.0;
// Predict new state
m_fusionQPose.setScalar(qs + (-x2 * qx - y2 * qy - z2 * qz) * m_timeDelta);
m_fusionQPose.setX(qx + (x2 * qs + z2 * qy - y2 * qz) * m_timeDelta);
m_fusionQPose.setY(qy + (y2 * qs - z2 * qx + x2 * qz) * m_timeDelta);
m_fusionQPose.setZ(qz + (z2 * qs + y2 * qx - x2 * qy) * m_timeDelta);
// update();
#ifdef USE_SLERP
if (m_enableCompass || m_enableAccel) {
// calculate rotation delta
m_rotationDelta = m_fusionQPose.conjugate() * m_measuredQPose;
m_rotationDelta.normalize();
// take it to the power (0 to 1) to give the desired amount of correction
RTFLOAT theta = acos(m_rotationDelta.scalar());
RTFLOAT sinPowerTheta = sin(theta * m_slerpPower);
RTFLOAT cosPowerTheta = cos(theta * m_slerpPower);
m_rotationUnitVector.setX(m_rotationDelta.x());
m_rotationUnitVector.setY(m_rotationDelta.y());
m_rotationUnitVector.setZ(m_rotationDelta.z());
m_rotationUnitVector.normalize();
m_rotationPower.setScalar(cosPowerTheta);
m_rotationPower.setX(sinPowerTheta * m_rotationUnitVector.x());
m_rotationPower.setY(sinPowerTheta * m_rotationUnitVector.y());
m_rotationPower.setZ(sinPowerTheta * m_rotationUnitVector.z());
m_rotationPower.normalize();
// multiple this by predicted value to get result
m_fusionQPose *= m_rotationPower;
}
#else
if (m_enableCompass || m_enableAccel) {
m_stateQError = m_measuredQPose - m_fusionQPose;
} else {
m_stateQError = RTQuaternion();
}
// make new state estimate
RTFLOAT qt = m_Q * m_timeDelta;
m_fusionQPose += m_stateQError * (qt / (qt + m_R));
#endif
m_fusionQPose.normalize();
m_fusionQPose.toEuler(m_fusionPose);
}
}
bool I2cImu::ImuSettings::loadSettings()
{
ROS_INFO("%s: reading IMU parameters from param server", __FUNCTION__);
int temp_int;
// General
settings_nh_->getParam("imu_type", m_imuType);
settings_nh_->getParam("fusion_type", m_fusionType);
if(settings_nh_->getParam("i2c_bus", temp_int))
m_I2CBus = (unsigned char) temp_int;
if(settings_nh_->getParam("i2c_slave_address", temp_int))
m_I2CSlaveAddress = (unsigned char) temp_int;
//double declination_radians;
//settings_nh_->param("magnetic_declination", declination_radians, 0.0);
//m_compassAdjDeclination = angles::to_degrees(declination_radians);
//MPU9150
settings_nh_->getParam("mpu9150/gyro_accel_sample_rate", m_MPU9150GyroAccelSampleRate);
settings_nh_->getParam("mpu9150/compass_sample_rate", m_MPU9150CompassSampleRate);
settings_nh_->getParam("mpu9150/accel_full_scale_range", m_MPU9150AccelFsr);
settings_nh_->getParam("mpu9150/gyro_accel_low_pass_filter", m_MPU9150GyroAccelLpf);
settings_nh_->getParam("mpu9150/gyro_full_scale_range", m_MPU9150GyroFsr);
//MPU9250
settings_nh_->getParam("mpu9250/gyro_accel_sample_rate", m_MPU9250GyroAccelSampleRate);
settings_nh_->getParam("mpu9250/compass_sample_rate", m_MPU9250CompassSampleRate);
settings_nh_->getParam("mpu9250/accel_full_scale_range", m_MPU9250AccelFsr);
settings_nh_->getParam("mpu9250/accel_low_pass_filter", m_MPU9250AccelLpf);
settings_nh_->getParam("mpu9250/gyro_full_scale_range", m_MPU9250GyroFsr);
settings_nh_->getParam("mpu9250/gyro_low_pass_filter", m_MPU9250GyroLpf);
//GD20HM303D
settings_nh_->getParam("GD20HM303D/gyro_sample_rate", m_GD20HM303DGyroSampleRate);
settings_nh_->getParam("GD20HM303D/accel_sample_rate", m_GD20HM303DAccelSampleRate);
settings_nh_->getParam("GD20HM303D/compass_sample_rate", m_GD20HM303DCompassSampleRate);
settings_nh_->getParam("GD20HM303D/accel_full_scale_range", m_GD20HM303DAccelFsr);
settings_nh_->getParam("GD20HM303D/gyro_full_scale_range", m_GD20HM303DGyroFsr);
settings_nh_->getParam("GD20HM303D/compass_full_scale_range", m_GD20HM303DCompassFsr);
settings_nh_->getParam("GD20HM303D/accel_low_pass_filter", m_GD20HM303DAccelLpf);
settings_nh_->getParam("GD20HM303D/gyro_high_pass_filter", m_GD20HM303DGyroHpf);
settings_nh_->getParam("GD20HM303D/gyro_bandwidth", m_GD20HM303DGyroBW);
//GD20M303DLHC
settings_nh_->getParam("GD20M303DLHC/gyro_sample_rate",m_GD20M303DLHCGyroSampleRate);
settings_nh_->getParam("GD20M303DLHC/accel_sample_rate",m_GD20M303DLHCAccelSampleRate);
settings_nh_->getParam("GD20M303DLHC/compass_sample_rate",m_GD20M303DLHCCompassSampleRate);
settings_nh_->getParam("GD20M303DLHC/accel_full_scale_range",m_GD20M303DLHCAccelFsr);
settings_nh_->getParam("GD20M303DLHC/gyro_full_scale_range",m_GD20M303DLHCGyroFsr);
settings_nh_->getParam("GD20M303DLHC/compass_full_scale_range",m_GD20M303DLHCCompassFsr);
settings_nh_->getParam("GD20M303DLHC/gyro_high_pass_filter",m_GD20M303DLHCGyroHpf);
settings_nh_->getParam("GD20M303DLHC/gyro_bandwidth",m_GD20M303DLHCGyroBW);
//GD20HM303DLHC
settings_nh_->getParam("GD20HM303DLHC/gyro_sample_rate", m_GD20HM303DLHCGyroSampleRate);
settings_nh_->getParam("GD20HM303DLHC/accel_sample_rate",m_GD20HM303DLHCAccelSampleRate);
settings_nh_->getParam("GD20HM303DLHC/compass_sample_rate",m_GD20HM303DLHCCompassSampleRate);
settings_nh_->getParam("GD20HM303DLHC/accel_full_scale_range",m_GD20HM303DLHCAccelFsr);
settings_nh_->getParam("GD20HM303DLHC/gyro_full_scale_range",m_GD20HM303DLHCGyroFsr);
settings_nh_->getParam("GD20HM303DLHC/compass_full_scale_range",m_GD20HM303DLHCCompassFsr);
settings_nh_->getParam("GD20HM303DLHC/gyro_high_pass_filter",m_GD20HM303DLHCGyroHpf);
settings_nh_->getParam("GD20HM303DLHC/gyro_bandwidth",m_GD20HM303DLHCGyroBW);
//LSM9DS0
settings_nh_->getParam("LSM9DS0/gyro_sample_rate",m_LSM9DS0GyroSampleRate);
settings_nh_->getParam("LSM9DS0/accel_sample_rate",m_LSM9DS0AccelSampleRate);
settings_nh_->getParam("LSM9DS0/compass_sample_rate",m_LSM9DS0CompassSampleRate);
settings_nh_->getParam("LSM9DS0/accel_full_scale_range",m_LSM9DS0AccelFsr);
settings_nh_->getParam("LSM9DS0/gyro_full_scale_range",m_LSM9DS0GyroFsr);
settings_nh_->getParam("LSM9DS0/compass_full_scale_range",m_LSM9DS0CompassFsr);
settings_nh_->getParam("LSM9DS0/accel_low_pass_filter",m_LSM9DS0AccelLpf);
settings_nh_->getParam("LSM9DS0/gyro_high_pass_filter",m_LSM9DS0GyroHpf);
settings_nh_->getParam("LSM9DS0/gyro_bandwidth",m_LSM9DS0GyroBW);
std::vector<int> compass_max, compass_min;
if (settings_nh_->getParam("calib/compass_min", compass_min)
&& settings_nh_->getParam("calib/compass_max", compass_max)
&& compass_min.size() == 3 && compass_max.size() == 3)
{
m_compassCalMin = RTVector3(compass_min[0], compass_min[1], compass_min[2]);
m_compassCalMax = RTVector3(compass_max[0],compass_max[1], compass_max[2]);
m_compassCalValid = true;
}
std::vector<int> accel_max, accel_min;
if (settings_nh_->getParam("calib/accel_min", accel_min)
&& settings_nh_->getParam("calib/accel_max", accel_max)
&& accel_min.size() == 3 && accel_max.size() == 3)
{
m_accelCalMin = RTVector3(accel_min[0], accel_min[1], accel_min[2]);
m_accelCalMax = RTVector3(accel_max[0],accel_max[1], accel_max[2]);
m_accelCalValid = true;
}
return true;
}
