加速規信號的零點校正

這一個問題緣起於我們希望對四旋翼直升機上的加速度信號做「零點校正」。

假設從 MPU6500 取得的加速度信號分別是 $x_i$, $y_i$, 以及 $z_i$，這三個軸未知零點的數值分別是 $x_0$, $y_0$, 以及 $z_0$，而且重力加速度 $g$ 不變。

如果我們量測三次，得到 $(x_1, y_1, z_1)$, $(x_2, y_2, z_2)$, $(x_3, y_3, z_3)$ 三組數據，那麼我們就可以有以下三個關係式
\[ (x_1-x_0)^2+(y_1-y_0)^2+(z_1-z_0)^2 = g^2, \] \[ (x_2-x_0)^2+(y_2-y_0)^2+(z_2-z_0)^2 = g^2, \] \[ (x_3-x_0)^2+(y_3-y_0)^2+(z_3-z_0)^2 = g^2. \]

第一式減去第二式可以得到
\[ x_1^2 + y_1^2 + z_1^2 - x_2^2 - y_2^2 - z_2^2 = 2(x_1-x_2)x_0 + 2(y_1-y_2)y_0 + 2(z_1-z_2)z_0 \]
第一式減去第三式可以得到
\[ x_1^2 + y_1^2 + z_1^2 - x_3^2 - y_3^2 - z_3^2 = 2(x_1-x_3)x_0 + 2(y_1-y_3)y_0 + 2(z_1-z_3)z_0 \]

第二式減去第三式可以得到
\[ x_2^2 + y_2^2 + z_2^2 - x_3^2 - y_3^2 - z_3^2 = 2(x_2-x_3)x_0 + 2(y_2-y_3)y_0 + 2(z_2-z_3)z_0 \]

寫成矩陣的形式
\[
\left[
\begin{array}
 .2(x_1-x_2) & 2(x_1-x_3) & 2(x_2-x_3) \\
 2(y_1-y_2) & 2(y_1-y_3) & 2(y_2-y_3) \\
 2(z_1-z_2) & 2(z_1-z_3) & 2(z_2-z_3)
\end{array}
\right]
\left[
\begin{array}
 .x_0 \\
 y_0 \\
 z_0
\end{array}
\right] =
\left[
\begin{array}
 .x_1^2 + y_1^2 + z_1^2 - x_2^2 - y_2^2 - z_2^2 \\
 x_1^2 + y_1^2 + z_1^2 - x_3^2 - y_3^2 - z_3^2 \\
 x_2^2 + y_2^2 + z_2^2 - x_3^2 - y_3^2 - z_3^2
\end{array}
\right]
\]

因此，這三個軸未知零點的數值 $x_0$, $y_0$, 以及 $z_0$，就可以利用以下的公式求出來，當然前提是反矩陣存在。
\[
\left[
\begin{array}
 .x_0 \\
 y_0 \\
 z_0
\end{array}
\right] =
\left[
\begin{array}
 .2(x_1-x_2) & 2(x_1-x_3) & 2(x_2-x_3) \\
 2(y_1-y_2) & 2(y_1-y_3) & 2(y_2-y_3) \\
 2(z_1-z_2) & 2(z_1-z_3) & 2(z_2-z_3)
\end{array}
\right]^{-1}
\left[
\begin{array}
 .x_1^2 + y_1^2 + z_1^2 - x_2^2 - y_2^2 - z_2^2 \\
 x_1^2 + y_1^2 + z_1^2 - x_3^2 - y_3^2 - z_3^2 \\
 x_2^2 + y_2^2 + z_2^2 - x_3^2 - y_3^2 - z_3^2
\end{array}
\right]
\]

只是從 MPU6500 取得的加速度信號通常很髒，因此 $(x_1, y_1, z_1)$, $(x_2, y_2, z_2)$, $(x_3, y_3, z_3)$ 這三組數據，究竟應該是濾波後的結果，還是不需要呢？另外，反矩陣的計算其實對寫程式而言不是很友善，因此如何利用其他的演算法(如 Recursive Least Square的方法)來實現，也是值得討論。