114 lines
3.3 KiB
C
114 lines
3.3 KiB
C
#include "imu/imu_calibration.h"
|
|
|
|
#include <math.h>
|
|
#include <stddef.h>
|
|
#include <string.h>
|
|
|
|
static bool vector_is_finite(const double vector[3])
|
|
{
|
|
return isfinite(vector[0]) && isfinite(vector[1]) && isfinite(vector[2]);
|
|
}
|
|
|
|
static bool matrix_is_finite_and_invertible(const double matrix[3][3])
|
|
{
|
|
for(unsigned int row = 0 ; row < 3u ; row++)
|
|
{
|
|
if(!vector_is_finite(matrix[row]))
|
|
{
|
|
return false;
|
|
}
|
|
}
|
|
double determinant =
|
|
matrix[0][0] * (matrix[1][1] * matrix[2][2] -
|
|
matrix[1][2] * matrix[2][1]) -
|
|
matrix[0][1] * (matrix[1][0] * matrix[2][2] -
|
|
matrix[1][2] * matrix[2][0]) +
|
|
matrix[0][2] * (matrix[1][0] * matrix[2][1] -
|
|
matrix[1][1] * matrix[2][0]);
|
|
return isfinite(determinant) && (fabs(determinant) > 1.0e-12);
|
|
}
|
|
|
|
void imu_calibration_default(imu_calibration_t* calibration)
|
|
{
|
|
if(calibration == NULL)
|
|
{
|
|
return;
|
|
}
|
|
memset(calibration, 0, sizeof(*calibration));
|
|
for(unsigned int axis = 0 ; axis < 3u ; axis++)
|
|
{
|
|
calibration->accel_transform[axis][axis] = 1.0;
|
|
calibration->gyro_transform[axis][axis] = 1.0;
|
|
}
|
|
calibration->reference_temperature_c = 25.0;
|
|
}
|
|
|
|
bool imu_calibration_is_valid(const imu_calibration_t* calibration)
|
|
{
|
|
if((calibration == NULL) ||
|
|
!vector_is_finite(calibration->accel_bias_mps2) ||
|
|
!vector_is_finite(calibration->gyro_bias_rps) ||
|
|
!matrix_is_finite_and_invertible(calibration->accel_transform) ||
|
|
!matrix_is_finite_and_invertible(calibration->gyro_transform) ||
|
|
!isfinite(calibration->reference_temperature_c) ||
|
|
!vector_is_finite(calibration->accel_temperature_coefficient_mps2_per_c) ||
|
|
!vector_is_finite(calibration->gyro_temperature_coefficient_rps_per_c))
|
|
{
|
|
return false;
|
|
}
|
|
return true;
|
|
}
|
|
|
|
bool imu_calibration_apply(const imu_calibration_t* calibration,
|
|
imu_sample_t* measurement)
|
|
{
|
|
if(!imu_calibration_is_valid(calibration) || (measurement == NULL) ||
|
|
!vector_is_finite(measurement->accel_mps2) ||
|
|
!vector_is_finite(measurement->gyro_rps))
|
|
{
|
|
return false;
|
|
}
|
|
double temperature_delta_c = 0.0;
|
|
if(calibration->temperature_compensation_enabled)
|
|
{
|
|
if(!isfinite(measurement->temperature_c))
|
|
{
|
|
return false;
|
|
}
|
|
temperature_delta_c =
|
|
measurement->temperature_c - calibration->reference_temperature_c;
|
|
}
|
|
|
|
double accel_unbiased[3];
|
|
double gyro_unbiased[3];
|
|
for(unsigned int axis = 0 ; axis < 3u ; axis++)
|
|
{
|
|
accel_unbiased[axis] =
|
|
measurement->accel_mps2[axis] - calibration->accel_bias_mps2[axis] -
|
|
calibration->accel_temperature_coefficient_mps2_per_c[axis] *
|
|
temperature_delta_c;
|
|
gyro_unbiased[axis] =
|
|
measurement->gyro_rps[axis] - calibration->gyro_bias_rps[axis] -
|
|
calibration->gyro_temperature_coefficient_rps_per_c[axis] *
|
|
temperature_delta_c;
|
|
}
|
|
|
|
double accel_corrected[3] = {0.0};
|
|
double gyro_corrected[3] = {0.0};
|
|
for(unsigned int row = 0 ; row < 3u ; row++)
|
|
{
|
|
for(unsigned int col = 0 ; col < 3u ; col++)
|
|
{
|
|
accel_corrected[row] +=
|
|
calibration->accel_transform[row][col] * accel_unbiased[col];
|
|
gyro_corrected[row] +=
|
|
calibration->gyro_transform[row][col] * gyro_unbiased[col];
|
|
}
|
|
}
|
|
memcpy(measurement->accel_mps2, accel_corrected,
|
|
sizeof(measurement->accel_mps2));
|
|
memcpy(measurement->gyro_rps, gyro_corrected,
|
|
sizeof(measurement->gyro_rps));
|
|
return true;
|
|
}
|