加速度センサーとは

加速度を検知するセンサー．
加速度とは，速度の微分(変化量)．
高校物理で習う方程式`F=ma`を思い出せば，物体に掛かっている力を検知するとも言えそう．
一番検知しやすいのは重力．センサーが重力の方向に対してどの程度傾いているのか検知する．

こちらの加速度センサーを使用．

3軸加速度センサーを手に入れたので軽く触る(arduino) - shonen.hateblo.jp

正確な値の測定

静止状態（加速していない状態）のセンサーの3軸の値の大きさを求めてみる．

この状態でセンサーが影響を受ける要素は重力のみである．

#include <SPI.h>
#include <Wire.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>

#define OLED_RESET 4
Adafruit_SSD1306 display(OLED_RESET);

#if (SSD1306_LCDHEIGHT != 64)
#error("Height incorrect, please fix Adafruit_SSD1306.h!");
#endif


void setup() {
  pinMode(A0, INPUT);
  pinMode(A1, INPUT);
  pinMode(A2, INPUT);
  
  Serial.begin(9600);

  display.begin(SSD1306_SWITCHCAPVCC, 0x3C);  // initialize with the I2C addr 0x3D (for the 128x64)
  
  display.display();
  delay(2000);

  display.setTextSize(1);
  display.setTextColor(WHITE);
}


void loop() {

  int32_t acc_x = analogRead(A0)-512;
  int32_t acc_y = analogRead(A1)-512;
  int32_t acc_z = analogRead(A2)-512;

  double magnitude = sqrt(acc_x*acc_x + acc_y*acc_y + acc_z*acc_z);

  display.clearDisplay();
  
  display.setCursor(0,0);
  display.println((magnitude));

  display.display();
}

向きを変えて静止させると，元の数字より大きくなったり，小さくなったりする．
なぜか？
上のプログラムでは，`512`をニュートラル値(力が全くかかっていない時の値)として扱っている．
しかし，実際のところは違う可能性がある．
- http://shonen.hateblo.jp/entry/2018/03/20/135715 の結果から，ずれがあることが実験的に分かっている．
まず，力が全くかかっていない時の値の推定をする必要がある．
- ニュートラル値を数値解析的に推定したい

パラメータの洗い出し

未知のパラメータは次の4つ

x軸方向に全く力がかかっていない時のセンサーの値 $X$
y軸方向に全く力がかかっていない時のセンサーの値 $Y$
z軸方向に全く力がかかっていない時のセンサーの値 $Z$
重力の大きさ $G$

測定出来るデータは次の3種類

静止した状態の時に測定したセンサーの値 $x_i, y_i, z_i$

測定データをいくつかかき集めて，未知パラメータを推定しよう，という事がしたいです．

方程式

N個の測定データを取得したとき、次のような連立方程式が成立します．

$(x_1-X)^2+(y_1-Y)^2+(z_1-Z)^2 \simeq G^2$
$(x_2-X)^2+(y_2-Y)^2+(z_2-Z)^2 \simeq G^2$
$\vdots$
$(x_N-X)^2+(y_N-Y)^2+(z_N-Z)^2 \simeq G^2$

最尤推定や最小二乗法を使って $X,Y,Z$ を推定すれば良いです．

最小二乗法

$G$ については興味が無いので，以降 $G:=G^2$ と置く．

上の連立方程式の『 $\simeq$ 』の誤差の二乗和 $J$ は次のようになる．

$J=\sum_{i=1}^{N}((x_i-X)^2+(y_i-Y)^2+(z_i-Z)^2-G)^2$

Jを最小化したいので，偏微分する．

$\frac{\delta J}{\delta X}=4\sum_{i=1}^{N}\{(X-x_i)^3+(X-x_i)((y_i-Y)^2+(z_i-Z)^2-G)\}$
$\frac{\delta J}{\delta Y}=4\sum_{i=1}^{N}\{(Y-y_i)^3+(Y-y_i)((x_i-X)^2+(z_i-Z)^2-G)\}$
$\frac{\delta J}{\delta Z}=4\sum_{i=1}^{N}\{(Z-z_i)^3+(Z-z_i)((x_i-X)^2+(y_i-Y)^2-G)\}$
$\frac{\delta J}{\delta G}=2\sum_{i=1}^{N}\{G-(x_i-X)^2-(y_i-Y)^2-(z_i-Z)^2\}$

偏微分値が0の時の $X,Y,Z,G$ を連立方程式として解きます．

方程式を解く

非線形の方程式なので，計算機に任せる方法を考えてみます．

ニュートン法を使います．ニュートン法の説明はしません．

二階偏微分を求めます．

$\frac{\delta}{\delta X}\frac{\delta J}{\delta X}=4\sum_{i=1}^{N}\{3(X-x_i)^2+(y_i-Y)^2+(z_i-Z)^2-G\}$
$\frac{\delta}{\delta Y}\frac{\delta J}{\delta X}=8\sum_{i=1}^{N}\{(X-x_i)(Y-y_i)\}$
$\frac{\delta}{\delta Z}\frac{\delta J}{\delta X}=8\sum_{i=1}^{N}\{(X-x_i)(Z-z_i)\}$
$\frac{\delta}{\delta G}\frac{\delta J}{\delta X}=4\sum_{i=1}^{N}\{(x_i-X)\}$
$\frac{\delta}{\delta X}\frac{\delta J}{\delta Y}=8\sum_{i=1}^{N}\{(Y-y_i)(X-x_i)\}$
$\frac{\delta}{\delta Y}\frac{\delta J}{\delta Y}=4\sum_{i=1}^{N}\{3(Y-y_i)^2+(x_i-X)^2+(z_i-Z)^2-G\}$
$\frac{\delta}{\delta Z}\frac{\delta J}{\delta Y}=8\sum_{i=1}^{N}\{(Y-y_i)(Z-z_i)\}$
$\frac{\delta}{\delta G}\frac{\delta J}{\delta Y}=4\sum_{i=1}^{N}\{(y_i-Y)\}$
$\frac{\delta}{\delta X}\frac{\delta J}{\delta Z}=8\sum_{i=1}^{N}\{(Z-z_i)(X-x_i)\}$
$\frac{\delta}{\delta Y}\frac{\delta J}{\delta Z}=8\sum_{i=1}^{N}\{(Z-z_i)(Y-y_i)\}$
$\frac{\delta}{\delta Z}\frac{\delta J}{\delta Z}=4\sum_{i=1}^{N}\{3(Z-z_i)^2+(x_i-X)^2+(y_i-Y)^2-G\}$
$\frac{\delta}{\delta G}\frac{\delta J}{\delta Z}=4\sum_{i=1}^{N}\{(z_i-Z)\}$
$\frac{\delta}{\delta X}\frac{\delta J}{\delta G}=4\sum_{i=1}^{N}\{(x_i-X)\}$
$\frac{\delta}{\delta Y}\frac{\delta J}{\delta G}=4\sum_{i=1}^{N}\{(y_i-Y)\}$
$\frac{\delta}{\delta Z}\frac{\delta J}{\delta G}=4\sum_{i=1}^{N}\{(z_i-Z)\}$
$\frac{\delta}{\delta G}\frac{\delta J}{\delta G}=2N$

ニュートン法の漸化式．
t回目の反復は次の式で表現できるので，これを実装してループを回せばよいです．

$\left(\begin{array}{c}X_{(t)}\\Y_{(t)}\\Z_{(t)}\\G_{(t)}\\\end{array}\right) = \left(\begin{array}{c}X_{(t-1)}\\Y_{(t-1)}\\Z_{(t-1)}\\G_{(t-1)}\\\end{array}\right)-\left( \begin{array}{cccc} \frac{\delta }{\delta X}\frac{\delta J}{\delta X} & \frac{\delta }{\delta Y}\frac{\delta J}{\delta X} & \frac{\delta }{\delta Z}\frac{\delta J}{\delta X} & \frac{\delta }{\delta G}\frac{\delta J}{\delta X} \\ \frac{\delta }{\delta X}\frac{\delta J}{\delta Y} & \frac{\delta }{\delta Y}\frac{\delta J}{\delta Y} & \frac{\delta }{\delta Z}\frac{\delta J}{\delta Y} & \frac{\delta }{\delta G}\frac{\delta J}{\delta Y} \\ \frac{\delta }{\delta X}\frac{\delta J}{\delta Z} & \frac{\delta }{\delta Y}\frac{\delta J}{\delta Z} & \frac{\delta }{\delta Z}\frac{\delta J}{\delta Z} & \frac{\delta }{\delta G}\frac{\delta J}{\delta Z} \\ \frac{\delta }{\delta X}\frac{\delta J}{\delta G} & \frac{\delta }{\delta Y}\frac{\delta J}{\delta G} & \frac{\delta }{\delta Z}\frac{\delta J}{\delta G} & \frac{\delta }{\delta G}\frac{\delta J}{\delta G} \\ \end{array} \right)^{-1}\left(\begin{array}{c}\frac{\delta J}{\delta X}\\\frac{\delta J}{\delta Y}\\\frac{\delta J}{\delta Z}\\\frac{\delta J}{\delta G}\\\end{array}\right)$

python3(numpy)を書きました．

from collections import namedtuple
import numpy

# 構造体っぽいの定義
Parameter = namedtuple("Parameter", "X Y Z G")
Record = namedtuple("Record", "x y z")


# 入力読み込み
N = int(input())
records = [Record(*map(float, input().split())) for i in range(N)]


param = Parameter(0, 0, 0, 0)

for loopCount in range(50):

    dx = dy = dz = dg = 0.0
    dxdx = dydx = dzdx = dgdx = 0.0
    dxdy = dydy = dzdy = dgdy = 0.0
    dxdz = dydz = dzdz = dgdz = 0.0
    dxdg = dydg = dzdg = dgdg = 0.0
    ja = 0.0

    for rec in records:
        ja += ((rec.x - param.X) ** 2 + (rec.y - param.Y) ** 2 + (rec.z - param.Z) ** 2 - param.G)**2

        dx += 4.0*((param.X - rec.x)**3 + (param.X - rec.x)*((rec.y - param.Y)**2 + (rec.z - param.Z)**2 - param.G))
        dy += 4.0*((param.Y - rec.y)**3 + (param.Y - rec.y)*((rec.x - param.X)**2 + (rec.z - param.Z)**2 - param.G))
        dz += 4.0*((param.Z - rec.z)**3 + (param.Z - rec.z)*((rec.x - param.X)**2 + (rec.y - param.Y)**2 - param.G))
        dg += 2.0*(param.G - ((rec.x - param.X) ** 2 + (rec.y - param.Y) ** 2 + (rec.z - param.Z) ** 2))

        dxdx += 4.0*(3.0*(param.X - rec.x)**2 + (param.Y - rec.y)**2 + (param.Z - rec.z)**2 - param.G)
        dydx += 8.0*(param.X - rec.x)*(param.Y - rec.y)
        dzdx += 8.0*(param.X - rec.x)*(param.Z - rec.z)
        dgdx += 4.0*(rec.x - param.X)

        dxdy += 8.0*(param.Y - rec.y)*(param.X - rec.x)
        dydy += 4.0*((param.X - rec.x)**2 + 3.0*(param.Y - rec.y)**2 + (param.Z - rec.z)**2 - param.G)
        dzdy += 8.0*(param.Y - rec.y)*(param.Z - rec.z)
        dgdy += 4.0*(rec.y - param.Y)

        dxdz += 8.0*(param.Z - rec.z)*(param.X - rec.x)
        dydz += 8.0*(param.Z - rec.z)*(param.Y - rec.y)
        dzdz += 4.0*((param.X - rec.x)**2 + (param.Y - rec.y)**2 + 3.0*(param.Z - rec.z)**2 - param.G)
        dgdz += 4.0*(rec.z - param.Z)

        dxdg += 4.0*(rec.x - param.X)
        dydg += 4.0*(rec.y - param.Y)
        dzdg += 4.0*(rec.z - param.Z)
        dgdg += 2.0

    # print(ja)
    # print(param)

    jaco = numpy.matrix(
        (
            [dxdx, dydx, dzdx, dgdx],
            [dxdy, dydy, dzdy, dgdy],
            [dxdz, dydz, dzdz, dgdz],
            [dxdg, dydg, dzdg, dgdg]
        ))

    ocaj = numpy.linalg.inv(jaco)

    pv = numpy.mat([param.X, param.Y, param.Z, param.G]).T - ocaj*numpy.mat([dx, dy, dz, dg]).T

    param = Parameter(pv[0, 0], pv[1, 0], pv[2, 0], pv[3, 0])
print(param)

入力データはこちら．arduinoから取得したデータで，静止時(姿勢は異なる)のセンサーの各軸の値です．

26
517 489 702
482 742 497
727 524 517
257 522 507
475 510 313
292 632 497
701 419 564
480 371 665
664 627 602
325 387 600
313 527 381
479 676 372
600 376 369
614 649 390
467 480 702
720 519 554
500 504 703
403 519 684
475 731 447
698 584 436
257 520 505
349 359 404
492 579 694
524 439 693
469 616 681
471 474 701

得たもの

Parameter(X=493.25812379306166, Y=511.6144020468419, Z=494.82229994669643, G=48703.524830696435)

結果をarduinoに組み込みます

void loop() {

  int32_t acc_x = analogRead(A0) - 493;
  int32_t acc_y = analogRead(A1) - 511;
  int32_t acc_z = analogRead(A2) - 495;

  double magnitude = sqrt(acc_x*acc_x + acc_y*acc_y + acc_z*acc_z);

  display.clearDisplay();
  
  display.setCursor(0,0);
  display.println(magnitude);

  display.display();
}

結果

X,Y軸方向に傾けた時に30程度重力が大きく表示される…．

おまけ

もう少し高精度な関数当てはめを考える．
各軸ごとの傾向きの感度(倍率)を導入する．新たに導入する変数を $A,B,C$ とすると，i番目の測定データは次の方程式を満たすような挙動をする，と仮定する．

$A(x_i-X)^2+B(y_i-Y)^2+C(z_i-Z)^2 \simeq G$

変数が7つに増えました．微分回数60回近く．また暇な時にやろうと思います．

shonen.hateblo.jp

やったこと，しらべたことを書く．

3軸加速度センサーのキャリブレーションを試みた(arduino)

加速度センサーとは

正確な値の測定

パラメータの洗い出し

方程式

最小二乗法

方程式を解く

結果

おまけ