加速規信號的零點校正

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

假設從 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的方法)來實現，也是值得討論。