AHRS Sensor Calibration

Accelerometer calibration

๊ฐ๋„ ์˜ค์ฐจ๋Š” ์—†๋‹ค๊ณ  ๊ฐ€์ •ํ•˜๊ณ  ์˜คํ”„์…‹๋งŒ ๊ณ„์‚ฐํ•ฉ๋‹ˆ๋‹ค.

  • ์„ผ์„œ์˜ ํ•œ ์ถ•์„ ์ง€ํ‘œ์— ์ˆ˜์ง์ธ ๋ฐฉํ–ฅ๊ณผ ๋‚˜๋ž€ํ•˜๊ฒŒ ๋งž์ถฅ๋‹ˆ๋‹ค.
  • ์›€์ง์ž„์ด๋‚˜ ์ง„๋™์ด ์ธก์ •๋˜์ง€ ์•Š๋„๋ก ์„ผ์„œ๋ฅผ ๊ณ ์ •ํ•ฉ๋‹ˆ๋‹ค.
  • ๊ฐ€์†๋„ ๊ฐ’์„ ์—ฌ๋Ÿฌ๋ฒˆ ์ธก์ •ํ•˜์—ฌ ํ‰๊ท ์„ ๊ตฌํ•ฉ๋‹ˆ๋‹ค.
  • ์ง€ํ‘œ์— ์ˆ˜์ง์ธ ๋ฐฉํ–ฅ๊ณผ ๋‚˜๋ž€ํ•œ ์ถ•์€ ์„ผ์„œ์˜ 1 g์— ํ•ด๋‹นํ•˜๋Š” ๊ฐ’๊ณผ ๋น„๊ตํ•˜์—ฌ ์˜คํ”„์…‹์„ ๊ตฌํ•ฉ๋‹ˆ๋‹ค.
  • ๋‹ค๋ฅธ ์ถ•์€ ์ธก์ •๋œ ํ‰๊ท  ๊ฐ’์ด ์˜คํ”„์…‹ ์ž…๋‹ˆ๋‹ค.

Example

HAL_StatusTypeDef mpu9250_acc_calibration(void) {
int16_t raw_acc[3];
int32_t raw_acc_sum[3] = {0};
HAL_StatusTypeDef status;
for(int16_t i = 0; i < 256; ++i) {
status = mpu9250_read_3_axis(MPU9250_ACCEL_XOUT_H, raw_acc);
if(status != HAL_OK) { return status; }
raw_acc_sum[0] += raw_acc[0];
raw_acc_sum[1] += raw_acc[1];
raw_acc_sum[2] += raw_acc[2];
delay_ms(2);
}
acc_offset[0] = raw_acc_sum[0] >> 8;
acc_offset[1] = raw_acc_sum[1] >> 8;
acc_offset[2] = raw_acc_sum[2] >> 8;
// An axis must be parallel to vertical.
for(int16_t i = 0; i < 3; ++i) {
if(acc_offset[i] > acc_1G_FS * 0.9) {
acc_offset[i] -= acc_1G_FS;
} else if(acc_offset[i] < -acc_1G_FS * 0.9) {
acc_offset[i] += acc_1G_FS;
}
}
return HAL_OK;
}

Gyroscope calibration

๊ฐ๋„ ์˜ค์ฐจ๋Š” ์—†๋‹ค๊ณ  ๊ฐ€์ •ํ•˜๊ณ  ์˜คํ”„์…‹๋งŒ ๊ณ„์‚ฐํ•ฉ๋‹ˆ๋‹ค.

  • ์›€์ง์ž„์ด๋‚˜ ์ง„๋™์ด ์ธก์ •๋˜์ง€ ์•Š๋„๋ก ์„ผ์„œ๋ฅผ ๊ณ ์ •ํ•ฉ๋‹ˆ๋‹ค.
  • ๊ฐ์†๋„ ๊ฐ’์„ ์—ฌ๋Ÿฌ๋ฒˆ ์ธก์ •ํ•˜์—ฌ ํ‰๊ท ์„ ๊ตฌํ•ฉ๋‹ˆ๋‹ค.
  • ์ธก์ •๋œ ํ‰๊ท  ๊ฐ’์ด ์˜คํ”„์…‹ ์ž…๋‹ˆ๋‹ค.

Example

HAL_StatusTypeDef mpu9250_gyro_calibration(void) {
int16_t raw_gyro[3];
int32_t raw_gyro_sum[3] = {0};
HAL_StatusTypeDef status;
for(int16_t i = 0; i < 256; ++i) {
status = mpu9250_read_3_axis(MPU9250_GYRO_XOUT_H, raw_gyro);
if(status != HAL_OK) { return status; }
raw_gyro_sum[0] += raw_gyro[0];
raw_gyro_sum[1] += raw_gyro[1];
raw_gyro_sum[2] += raw_gyro[2];
delay_ms(2);
}
gyro_offset[0] = raw_gyro_sum[0] >> 8;
gyro_offset[1] = raw_gyro_sum[1] >> 8;
gyro_offset[2] = raw_gyro_sum[2] >> 8;
return HAL_OK;
}

Magnetometer calibration

์˜คํ”„์…‹๋งŒ ์žˆ๋‹ค๊ณ  ๊ฐ€์ •ํ•˜๋Š” ๊ฒฝ์šฐ, ๊ฐ๋„ ์˜ค์ฐจ์™€ ์˜คํ”„์…‹์ด ์žˆ๋‹ค๊ณ  ๊ฐ€์ •ํ•˜๋Š” ๊ฒฝ์šฐ๋งŒ ์ƒ๊ฐํ•ด์„œ ๊ณ„์‚ฐํ•ฉ๋‹ˆ๋‹ค. ๋‘ ๊ฒฝ์šฐ ๋ชจ๋‘ ์ตœ์†Œ์ž์Šน๋ฒ•(least squares method)์„ ์‚ฌ์šฉํ•˜๊ฒŒ ๋ฉ๋‹ˆ๋‹ค.

์ž๊ธฐ์žฅ์˜ ๊ฒฝ์šฐ ์œ„์น˜์™€ ์‹œ๊ฐ„์— ๋”ฐ๋ผ ๋ณ€ํ•˜์ง€๋งŒ ์ธก์ •ํ•˜๋Š” ๋™์•ˆ์—๋Š” ๋ณ€ํ•˜์ง€ ์•Š๋Š”๋‹ค๊ณ  ๊ฐ€์ •ํ•ฉ๋‹ˆ๋‹ค.

์„ผ์„œ๋ฅผ ์—ฌ๋Ÿฌ๋ฐฉํ–ฅ์œผ๋กœ ํšŒ์ „์‹œํ‚ค๋ฉฐ ๋ฐ์ดํ„ฐ๋ฅผ ์ธก์ •ํ•  ๋•Œ ์ƒ๊ธฐ๋Š” ๊ทธ๋ž˜ํ”„๋ฅผ ๊ตฌ์ฒด๋ผ๊ณ  ๊ฐ€์ •ํ•˜๋ฉด ์˜คํ”„์…‹๋งŒ ์žˆ๋Š” ๊ฒฝ์šฐ์ด๊ณ , ํšŒ์ „ํ•˜์ง€ ์•Š์€ ํƒ€์›์ฒด๋ผ๊ณ  ๊ฐ€์ •ํ•˜๋ฉด ๊ฐ๋„ ์˜ค์ฐจ์™€ ์˜คํ”„์…‹์ด ์žˆ๋Š” ๊ฒฝ์šฐ์ž…๋‹ˆ๋‹ค.

๊ตฌ์ฒด๋ผ๊ณ  ๊ฐ€์ •ํ•œ ๊ฒฝ์šฐ ์‹์€ ์•„๋ž˜์™€ ๊ฐ™์Šต๋‹ˆ๋‹ค.

mx2+my2+mz2+amx+bmy+cmz=dm^2_x + m^2_y + m^2_z + am_x + bm_y + cm_z = d
amx+bmy+cmzโˆ’d=โˆ’(mx2+my2+mz2)am_x + bm_y + cm_z - d = -(m^2_x + m^2_y + m^2_z)
(mxโˆ’โˆ’a2)2+(myโˆ’โˆ’b2)2+(mzโˆ’โˆ’c2)2=d+(a2)2+(b2)2+(c2)2\left(m_x - {-a \over 2}\right)^2 + \left(m_y - {-b \over 2}\right)^2 + \left(m_z - {-c \over 2}\right)^2 = d + \left( a \over 2 \right)^2 + \left( b \over 2 \right)^2 + \left( c \over 2 \right)^2

ํšŒ์ „ํ•˜์ง€ ์•Š์€ ํƒ€์›์ฒด๋ผ๊ณ  ๊ฐ€์ •ํ•œ ๊ฒฝ์šฐ ์‹์€ ์•„๋ž˜์™€ ๊ฐ™์Šต๋‹ˆ๋‹ค.

amx2+bmy2+cmz2+dmx+emy+fmz=1am^2_x+bm^2_y+cm^2_z+dm_x+em_y+fm_z=1
(mxโˆ’โˆ’d2a)2+(myโˆ’โˆ’e2bab)2+(mzโˆ’โˆ’f2cac)2=1000a+(d2a)2+ba(e2b)2+ca(f2c)2\begin{aligned} \left( m_x - {-d \over 2a} \right)^2 + \left( m_y - {-e \over 2b} \over \sqrt {a \over b} \right)^2 & + \left( m_z - {-f \over 2c} \over \sqrt {a \over c} \right)^2 \\ & = {1000 \over a} +\left( d \over 2a \right)^2 + {b \over a}\left( e \over 2b \right)^2 + {c \over a}\left( f \over 2c \right)^2 \end{aligned}

Least squares method

์ˆ˜์‹ ์ „๊ฐœ๋Š” ํšŒ์ „ํ•˜์ง€ ์•Š์€ ํƒ€์›์ฒด ๊ธฐ์ค€์œผ๋กœ ํ•˜๊ฒ ์Šต๋‹ˆ๋‹ค.

nํšŒ ์ธก์ •๋œ ๋ฐ์ดํ„ฐ๋ฅผ ํ–‰๋ ฌ์‹์œผ๋กœ ๋‚˜ํƒ€๋‚ด๋ฉด ์‹ (1)๊ณผ ๊ฐ™์Šต๋‹ˆ๋‹ค.

Mx=[mx,12my,12mz,12mx,1my,1mz,1mx,22my,22mz,22mx,2my,2mz,2โ‹ฎโ‹ฎโ‹ฎโ‹ฎโ‹ฎโ‹ฎโ‹ฎโ‹ฎโ‹ฎโ‹ฎโ‹ฎโ‹ฎโ‹ฎโ‹ฎโ‹ฎโ‹ฎโ‹ฎโ‹ฎmx,n2my,n2mz,n2mx,nmy,nmz,n][abcdef]=[11โ‹ฎโ‹ฎโ‹ฎ1]=c(1)Mx= \left[ \begin{matrix} m^2_{x,1} & m^2_{y,1} & m^2_{z,1} & m_{x,1} & m_{y,1} & m_{z,1} \\ m^2_{x,2} & m^2_{y,2} & m^2_{z,2} & m_{x,2} & m_{y,2} & m_{z,2} \\ \vdots & \vdots & \vdots & \vdots & \vdots & \vdots \\ \vdots & \vdots & \vdots & \vdots & \vdots & \vdots \\ \vdots & \vdots & \vdots & \vdots & \vdots & \vdots \\ m^2_{x,n} & m^2_{y,n} & m^2_{z,n} & m_{x,n} & m_{y,n} & m_{z,n} \end{matrix} \right] \left[ \begin{matrix} a \\ b \\ c \\ d \\ e \\ f \end{matrix} \right] = \left[ \begin{matrix} 1 \\ 1 \\ \vdots \\ \vdots \\ \vdots \\ 1 \end{matrix} \right] = c \tag{1}

xx๋ฅผ ๊ตฌํ•ด์•ผ ํ•˜๋Š”๋ฐ, MM์˜ ์—ญํ–‰๋ ฌ์„ ๊ตฌํ•  ์ˆ˜ ์—†๊ธฐ ๋•Œ๋ฌธ์— ์ตœ์†Œ์ž์Šน๋ฒ•์„ ์‚ฌ์šฉํ•˜๊ฒŒ ๋ฉ๋‹ˆ๋‹ค. ์ตœ์†Œ์ž์Šน๋ฒ•์€ ์˜ค์ฐจ ์ œ๊ณฑ์˜ ํ•ฉ์„ ์ตœ์†Œํ™” ํ•˜๋Š” ๊ฒƒ์ด๋ฏ€๋กœ ์ˆ˜์‹์€ ์•„๋ž˜์™€ ๊ฐ™์ด ์œ ๋„ํ•  ์ˆ˜ ์žˆ์Šต๋‹ˆ๋‹ค.

โˆ‚โˆ‚c(โˆฅcโˆ’Mxโˆฅ2)=โˆ’2MT(cโˆ’Mx)=0{\partial\over\partial c} \left( \begin{Vmatrix} c-Mx \end{Vmatrix}^2 \right) = -2M^T \left( c - Mx \right) = 0
MTMx=MTc(2)M^T Mx = M^T c \tag{2}
MTM=โˆ‘k=1n[mx,k4mx,k2my,k2mx,k2mz,k2mx,k3mx,k2my,kmx,k2mz,kmy,k4my,k2mz,k2my,k2mx,kmy,k3my,k2mz,kmz,k4mz,k2mx,kmz,k2my,kmz,k3mx,k2mx,kmy,kmx,kmz,ksymmetricmy,k2my,kmz,kmz,k2]M^TM= \sum_{k=1}^n \left[ \begin{matrix} m^4_{x,k} & m^2_{x,k}m^2_{y,k} & m^2_{x,k}m^2_{z,k} & m^3_{x,k} & m^2_{x,k}m_{y,k} & m^2_{x,k}m_{z,k} \\ & m^4_{y,k} & m^2_{y,k}m^2_{z,k} & m^2_{y,k}m_{x,k} & m^3_{y,k} & m^2_{y,k}m_{z,k} \\ & & m^4_{z,k} & m^2_{z,k}m_{x,k} & m^2_{z,k}m_{y,k} & m^3_{z,k} \\ & & & m^2_{x,k} & m_{x,k}m_{y,k} & m_{x,k}m_{z,k} \\ &symmetric& & & m^2_{y,k} & m_{y,k}m_{z,k} \\ & & & & & m^2_{z,k} \end{matrix} \right]
MTc=โˆ‘k=1n[mx,k2my,k2mz,k2mx,kmy,kmz,k]TM^Tc= \sum_{k=1}^n \left[ \begin{matrix} m^2_{x,k} & m^2_{y,k} & m^2_{z,k} & m_{x,k} & m_{y,k} & m_{z,k} \end{matrix} \right]^T
x=(MTM)โˆ’1MTc(3)x = \left( M^T M \right)^{-1} M^T c \tag{3}

์‹ (3)์„ ์‚ฌ์šฉํ•˜์—ฌ ๊ณ„์ˆ˜๋ฅผ ๊ตฌํ•  ์ˆ˜ ์žˆ์Šต๋‹ˆ๋‹ค.

Cholesky decomposition

์‹ (3)์„ ๊ณ„์‚ฐํ•˜๊ธฐ ์œ„ํ•ด ์—ญํ–‰๋ ฌ์„ ๊ตฌํ•ด์•ผํ•ฉ๋‹ˆ๋‹ค. ์—ญํ–‰๋ ฌ ๊ณ„์‚ฐ์„ ํ”ผํ•˜๊ธฐ ์œ„ํ•ด Cholesky decomposition์„ ์‚ฌ์šฉํ•˜๊ฒŒ ๋ฉ๋‹ˆ๋‹ค. ๋ถ„ํ•ด ์•Œ๊ณ ๋ฆฌ์ฆ˜์€ Choleskyโ€“Banachiewicz algorithm์„ ์‚ฌ์šฉํ–ˆ์Šต๋‹ˆ๋‹ค.

tip

Cholesky decomposition ์™ธ์— QR decomposition, SVD decomposition, Gaussian elimination ๋“ฑ ๋‹ค์–‘ํ•œ ๋ฐฉ๋ฒ•์„ ์‚ฌ์šฉํ•  ์ˆ˜ ์žˆ์Šต๋‹ˆ๋‹ค.

MTM=LLโˆ—(Choleskyย decomposition)M^T M = L L^*\text{(Cholesky decomposition)}
LLโˆ—x=Ly=MTc(4)L L^*x = Ly = M^T c \tag{4}
Lโˆ—x=y(5)L^*x=y \tag{5}

์‹ (4)์— forward substitution์„ ์ ์šฉํ•˜์—ฌ yy๋ฅผ ๊ตฌํ•˜๊ณ , ์‹ (5)์— backward substitution์„ ์ ์šฉํ•˜๋ฉด xx๋ฅผ ๊ตฌํ•  ์ˆ˜ ์žˆ์Šต๋‹ˆ๋‹ค.

๊ต์ • ์ „ ํ›„ ์ธก์ •๋œ ๋ฐ์ดํ„ฐ ๊ทธ๋ž˜ํ”„์ž…๋‹ˆ๋‹ค. ํŒŒ๋ž€์ƒ‰ ๋ฐ์ดํ„ฐ๊ฐ€ ๊ต์ • ์ „, ๋นจ๊ฐ„์ƒ‰ ๋ฐ์ดํ„ฐ๊ฐ€ ๊ต์ • ํ›„ ์ž…๋‹ˆ๋‹ค.

Examples

#include <math.h>
HAL_StatusTypeDef mpu9250_mag_calibration(void) {
HAL_StatusTypeDef status;
int16_t raw_mag[3];
uint8_t temp[6];
double component[6];
double sigma_A[21] = {0};
double sigma_b[6] = {0};
// M^t * M * coef = M^t * 1[k:1]
// sA * coef = sb
// ax^2 + by^2 + cz^2 + dx + ey + fz = 1
for(int16_t k = 0; k < 100; ++k) {
status = mpu9250_read_3_axis(MPU9250_EXT_SENS_DATA_00, raw_mag);
if(status != HAL_OK) { return status; }
component[3] = raw_mag[0];
component[4] = raw_mag[1];
component[5] = raw_mag[2];
component[0] = component[3] * component[3];
component[1] = component[4] * component[4];
component[2] = component[5] * component[5];
// Lower triangular matrix
for(int16_t i = 0; i < 6; ++i) {
temp[i] = i * (i + 1) >> 1; // row
for(int16_t j = 0; j < i + 1; ++j) {
sigma_A[temp[i] + j] += component[i] * component[j];
}
sigma_b[i] += component[i];
}
delay_ms(200);
}
// Cholesky decomposition
// sA = L * L^t
for(int16_t i = 0; i < 6; ++i) {
for(int16_t j = 0; j < i + 1; ++j) {
if(i == j) {
sigma_A[temp[i] + i] = sqrt(sigma_A[temp[i] + i]);
} else {
for(int16_t k = 0; k < j; ++k) {
sigma_A[temp[i] + j]
-= sigma_A[temp[i] + k] * sigma_A[temp[j] + k];
}
sigma_A[temp[i] + j] /= sigma_A[temp[j] + j];
sigma_A[temp[i] + i]
-= sigma_A[temp[i] + j] * sigma_A[temp[i] + j];
}
}
}
// L * ( L^t * coef ) = b
// L * x = b
// component == x
for(int16_t i = 0; i < 6; ++i) {
for(int16_t j = 0; j < i; ++j) {
sigma_b[i] -= sigma_A[temp[i] + j] * component[j];
}
component[i] = sigma_b[i] / sigma_A[temp[i] + i];
}
// L^t * coef = x
// component == x
// sigma_b == coef
for(int16_t i = 5; i >= 0; --i) {
for(int16_t j = 5; j > i; --j) {
component[i] -= sigma_A[temp[j] + i] * sigma_b[j];
}
sigma_b[i] = component[i] / sigma_A[temp[i] + i];
}
mag_offset[0] = (-sigma_b[3] / sigma_b[0]) / 2;
mag_offset[1] = (-sigma_b[4] / sigma_b[1]) / 2;
mag_offset[2] = (-sigma_b[5] / sigma_b[2]) / 2;
mag_sensitivity[0] = 1;
mag_sensitivity[1] = sqrt(sigma_b[1] / sigma_b[0]);
mag_sensitivity[2] = sqrt(sigma_b[2] / sigma_b[0]);
return HAL_OK;
}
Last updated on