楕円フィッティング¶

地磁気センサや加速度センサのキャリブレーションのために、与えられたデータセットに対して最もフィットする楕円を知りたくなることがある。そこで、最小二乗法を用いて楕円パラメータを推定することを考える。このとき、楕円とデータセットの残差をどのように定義すればよいか？という問題に直面した。結論としては、各データと、フィッティング楕円のマハラノビスノルムの差を残差として定義し、最小化すれば最尤楕円が求まることが分かった。

本ノートはそのメモである。

前提の整理 (センサキャリブレーションの方法/モデル定義、楕円の定義)
二次元の場合のフィッティング理論
三次元の場合のフィッティング理論
実データを用いたキャリブレーション実験

import matplotlib.pyplot as plt
from matplotlib.pyplot import *
import numpy as np
from numpy import *
import numpy.matlib

def compose(Vinv,th): # get representation matrix
    R=np.array([[np.cos(th),-np.sin(th)],[np.sin(th),np.cos(th)]])
    return R@Vinv@R.T

def decompose(Qinv): # representation mat ->  Vinv,Rot(th)
    lam,R=np.linalg.eig(Qinv)
    Vinv=np.diag(lam)
    return (Vinv,R)

def icov2implicit2(Q,chi2=1,offset=np.array([[0],[0]])):
    return lambda x1,x2 : Q[0,0]*((x1-offset[0])**2)+Q[1,1]*((x2-offset[1])**2) \
                     +2*Q[0,1]*((x1-offset[0])*(x2-offset[1]))+ -chi2

def abc2elip(a,b,chi2=1,resol=100,offset=np.array([[0],[0]])): #ax+by=chi^2
    th=np.linspace(-np.pi,np.pi,resol)
    x=np.c_[np.sqrt(chi2/a)*np.cos(th),np.sqrt(chi2/b)*np.sin(th)].T
    return (x+offset,np.diag([a,b]))

def abcth2elip(a,b,th,chi2=1,resol=100,offset=np.array([[0],[0]])):# th=radian
    x,Vinv=abc2elip(a,b,chi2,resol)
    R=np.array([[np.cos(th),-np.sin(th)],[np.sin(th),np.cos(th)]])
    return (R@x+ offset,Vinv,R,compose(Vinv,th))

def ivar2elip(Vinv,chi2=1,resol=100,offset=np.array([[0],[0]])): # (x^T)Qx=chi2  V=diag(var_x1,var_x2) //Quadratic form
    x,_=abc2elip(Vinv[0,0],Vinv[1,1],chi2,resol,offset)
    return x
    
def icov2elip(Qinv,chi2=1,resol=100,offset=np.array([[0],[0]])): # (x^T)Qx=chi2  Q=Cov(x)//Quadratic form
    Vinv,R=decompose(Qinv)
    x=ivar2elip(Vinv,chi2,resol)
    return (R@x+ offset , Vinv,R)

def qconj(q):
    q2=np.r_[q[0],-q[1]]
    return q2

def q2R(q):
    q2=bs.vec(np.r_[-q[1],q[0]])
    qR=np.c_[q,q2]
    return qR
    
def qpls(q1,q2):
    q3=q2R(q2)@q1
    return q3

def qdev(qtgt,qcur):
    return qpls(qtgt,qconj(qcur))

def th2q(th):
    return bs.vec(np.array([math.cos(th),math.sin(th)]))

def q2th(q):
    return math.atan2(q[1],q[0])

def qmean(qlist): # ave_(n) = {(n-1)/n)}*ave_(n-1) + (1/n)*x_(n)
    an=np.ones((2,1))
    for i,q in enumerate(qlist):
        j=i+1
        an=qslerp(an,q,1/j)
    return an
    
def qslerp(qs,qe,t): # spherical linear interpolation, return qe@t=1 qs@t=0
    th=q2th(qdev(qe,qs))
    return (math.sin((1-t)*th)*qs + math.sin(t*th)*qe )/(math.sin(th))
    

# th interfaces (deg,rad compatible)
def dev(thtgt,thcur,radian=True):
    f=1 if radian is True else d2r(1)    
    dq=qdev(th2q(f*thtgt),th2q(f*thcur))
    return q2th(dq)/f

def pls(th1,th2,radian=True):
    f=1 if radian is True else d2r(1)
    qp=qpls(th2q(f*th1),th2q(f*th2))
    return q2th(qp)/f

def mean(thlist,radian=True):
    f=1 if radian is True else d2r(1)
    f=d2r(1)
    qlist=[th2q(f*th) for th in thlist]
    qave=qmean(qlist)
    return q2th(qave)/f

# 
def pi2pi(th):
    r= math.atan2(math.sin(th),math.cos(th))
    return r
d2r = lambda d : d*math.pi/180
r2d = lambda r : r*180/math.pi

1. 前提の整理¶

1.1 想定するキャリブレーション方法¶

対象：地磁気センサ・加速度センサ

方針：地磁気ベクトル・重力加速度ベクトルのノルムが一定であることを利用する

方法：

センサを様々な方向に回転させ、データを集める(加速度センサについては、なるべく重力加速度のみを検出するようにゆっくり回転させる)
そのデータを3D散布図上にプロットすれば、理想的には原点を中心とする球体が描かれる。
しかし実際には楕円体が描かれる。
その楕円体を数式で表すと、どのような楕円体か？を推定する。
楕円体を理想の球体に変換するような座標変換を求める。

楕円体モデル：任意の点を中心とする傾いた楕円体

任意の点を中心とする (各軸の観測値に対してoffsetを仮定)
傾いた (各軸が厳密には直交していないことを仮定)
楕円体 (各軸の観測値に対して、+方向/-方向それぞれ同一のgainを仮定)

1.2 楕円の基本数式¶

楕円は以下のように二次形式で表現できる。

$$ \boldsymbol{x}^\mathrm{T} \boldsymbol{Q} \boldsymbol{x} =\chi^2 $$

ここで、$\boldsymbol{Q}$は表現行列と呼ばれる実対称行列である。

また、左辺をベクトル$\boldsymbol{x}$から実数$\chi^2$を出力する関数 $f:\mathbb{R}^{n}\rightarrow\mathbb{R}^{1}$と考え、マハラノビスノルムと定義する。

このとき、楕円はマハラノビスノルム=const.であるような位置ベクトルの集合であると考えることができる。

def MahNorm(Q,x):
    return np.diag(x.T@Q@x)[np.newaxis,:]

もし表現行列が以下のように対角化できたとする。

$$ \boldsymbol{P}^{-1}\boldsymbol{Q}\boldsymbol{P} = \boldsymbol{A} $$

このとき$\boldsymbol{P}$は直交行列であり、$\boldsymbol{P}^{-1} = \boldsymbol{P}^{T}$が成り立つ。 ∵表現行列$\boldsymbol{Q}$が実対称行列

対角化に用いた行列は以下の意味を持つ。

$\boldsymbol{A}$ : 楕円の各軸方向の拡大率を対角要素に持つ対角行列
$\boldsymbol{P}$ : 楕円の回転行列

参考

https://akeblo.blogspot.jp/2016/11/blog-post_46.html

2. 二次元でのシミュレーション¶

本章は以下の構成で進める

正解データとして楕円を用意
正解楕円+ノイズ　= センサから得られる観測値をシミュレーション
観測値に最小二乗法を適用 = 正解楕円を推定
推定楕円と正解楕円を比較

2.1 正解データの用意¶

まずは正解データとして適当な楕円を定義する。

今回は地磁気の大きさが460[mG]であることから、マハラノビスノルム=0.46となるような楕円を用意した。

なお、正解データの描画に使用している点数=シミュレーションされる観測数=100点である。

##reference data
para_ref=np.array([1.1,0.8,0.1,0.2,30])[:,np.newaxis]#rx,ry,cx,cy,th
n=100
chi2=0.46
idvec = np.arange(n)[np.newaxis,:]
def conf_para2elp(n,chi2):
    def para2elp(param):
        return abcth2elip(param[0,0],param[1,0],d2r(param[4,0]),resol=n,offset=param[2:4],chi2=chi2)
    return para2elp
para2elp = conf_para2elp(n,chi2)

data_ref,Aref,Pref,Qref=para2elp(para_ref)

plt.figure()
plt.plot(data_ref[0,:],data_ref[1,:],'r-',label="ref")
plt.axis("square")
plt.axis("equal")
plt.legend()
plt.xlabel("x")
plt.ylabel("y")
plt.grid()
plt.show()

正解データについて、マハラノビスノルムが一定であることを確認する。

nrm_ref=MahNorm(Qref, data_ref -np.matlib.repmat(para_ref[2:4],1,n))

plt.figure()
plt.plot(nrm_ref.ravel(),'r-',label="ref")
plt.ylim([.4,.5])
plt.legend()
plt.xlabel("data id")
plt.ylabel("MahNorm")
plt.grid()
plt.show()

2.2 観測データのシミュレーション¶

正解データに適当なノイズを乗せ、観測データを模擬する。

ノイズはホワイトノイズを仮定し、正規分布とした。

data=data_ref+np.random.randn(2,n)*0.05

plt.figure()
plt.plot(data_ref[0,:],data_ref[1,:],'r-',label="ref")
plt.plot(data[0,:],data[1,:],'y.',label="data")
plt.axis("square")
plt.axis("equal")
plt.legend()
plt.xlabel("x")
plt.ylabel("y")
plt.grid()
plt.show()

2.2 最小二乗法による楕円推定¶

2.1.1 定式化¶

最小二乗法を適用するためには、残差を定義する必要がある。なぜなら、残差二乗和を最小化したいから。

よくある線形近似と同じような考え方だと、「ある観測データのy」と「楕円モデルに観測データのxを代入して得られるy」の差分を残差として定義したくなる。

しかし、楕円の場合この考えは適切でない。なぜなら1つのxに対して2つのyが定まるから。

そこで、本ノートでは以下を残差として定義し、その二乗和を最小化することを考える。

残差 = 「観測データのマハラノビスノルム」と「正解楕円のマハラノビスノルム」の差

正解楕円のマハラノビスノルムは常に一定値(地磁気で言えば0.46)なのが新鮮。

定式化すると以下の$\hat{\boldsymbol{x}}$を求める非線形最小化問題になる。

$$ \hat{\boldsymbol{x}} = argmin(\boldsymbol{x} \in \mathbb{R}^{m\times1}) : \boldsymbol{\varepsilon}^{\mathrm{T}}\boldsymbol{\varepsilon} $$

$\boldsymbol{\varepsilon} \in \mathbb{R}^{n\times 1}$ : 残差ベクトル

残差ベクトルは以下の観測モデルによって定義される。

$$\boldsymbol{y}=F \left( \boldsymbol{x} \right) + \boldsymbol{\varepsilon}$$

$\boldsymbol{y} \in \mathbb{R}^{n\times 1}$ : すべての要素が理想楕円のマハラノビスノルム(ex.0.46)であるようなベクトル
$F : \mathbb{R}^{n\times m}\rightarrow\mathbb{R}^{n} $ : 観測データからマハラノビスノルムを算出する関数
$\boldsymbol{x} \in \mathbb{R}^{m\times 1}$ : 中心位置, 楕円拡大率, 回転角度
$n$ : 観測データ数
$m$ : 楕円パラメータ数

def sumsq(x):
    return x.T@x

def conf_model(data,n):
    def model(x):
        _,_,_,Q=para2elp(x)
        nrm=MahNorm(Q, data -np.matlib.repmat(x[2:4],1,n))
        return nrm.T
    return model


F = conf_model(data,n)
y  = np.ones(nrm_ref.shape).T*chi2

正解楕円のパラメータを用いて、データセットのマハラノビスノルムを計算し、以下に描画する。

この図の赤線と黄色線の差が残差である。

nrm=F(para_ref)

plt.figure()
plt.plot(nrm.ravel(),'y.-',label="data")
plt.plot(nrm_ref.ravel(),'r-',label="ref")
plt.xlabel("data id")
plt.ylabel("MahNorm")
plt.legend()
plt.grid()
plt.show()

実際には正解楕円のパラメータが分からない。

仮に適当な楕円パラメータを仮定すると、正解パラメータよりも残差が大きくなることが確認できる。

x0 = np.array([0.4,5.0,0.7,0.1,20])[:,np.newaxis]# rx,ry,cx,cy,th
pts0,_,_,_=para2elp(x0)
nrm0=F(x0)

plt.figure()
plt.plot(pts0[0,:],pts0[1,:],'c',label="x0")
plt.plot(data_ref[0,:],data_ref[1,:],'r-',label="ref")
plt.plot(data[0,:],data[1,:],'y.',label="data")
plt.axis("square")
plt.axis("equal")
plt.legend()
plt.xlabel("x")
plt.ylabel("y")
plt.grid()

plt.figure()
plt.plot(nrm.ravel(),'y.-',label="data")
plt.plot(nrm_ref.ravel(),'r-',label="ref")
plt.plot(nrm0.ravel(),'c.-',label="x0")
plt.xlabel("data id")
plt.ylabel("MahNorm")
plt.legend()
plt.grid()

plt.show()

2.2.2 解く¶

定式化が済んだので後は機械的に解いていく。

解法は以下

$$ \hat{\boldsymbol{x}} = \boldsymbol{x}_{N} $$

$\boldsymbol{x}_{i+1} = \boldsymbol{x}_i + \alpha \Delta \boldsymbol{x}_i$
$\boldsymbol{x}_{0} $ : 任意の初期値。正解値に近いことが望ましい。
$N$ : 反復回数
$\alpha$ : 学習率

また、$\Delta \boldsymbol{x}_{i}$は以下で与えらえる。

$$\Delta \boldsymbol{x}_{i} = \left( \boldsymbol{J}_i^\mathrm{T}\boldsymbol{J}_i \right)^{-1}\boldsymbol{J}_i^\mathrm{T} (\boldsymbol{y} - \boldsymbol{F}(\boldsymbol{x}_i))$$

ここで$\boldsymbol{J}_i$はヤコビアンであり、以下で与えられる。 $$\boldsymbol{J}_i = \left. \frac{\partial \boldsymbol{F}(\boldsymbol{x})}{\partial \boldsymbol{x}} \right|_{\boldsymbol{x}=\boldsymbol{x}_0}$$

参考：

https://akeblo.blogspot.jp/2016/11/blog-post_23.html

以下計算。

ヤコビアン求値には数値微分を用いた。
反復回数/学習率は適当に固定値とした。

def Jacob(x0):
    J=np.zeros((n,5))*np.NAN
    accr=0.001
    H=np.eye(5)*accr
    for i in np.arange(5):
        h=H[:,i]
        h=h[:,np.newaxis]
        J[:,i] = ( (F(x0+h)-F(x0))/accr  ).ravel()
    return J

itr=50
X=np.zeros((5,itr))
sq=np.zeros((1,itr))
for i in range(itr):
    dy=y-F(x0)
    J=Jacob(x0)
    dx=np.linalg.solve(J.T@J,J.T@dy)
    x0=x0+dx*0.15

    X[:,i]=x0.ravel()
    sq[:,i] =sumsq(dy)

描画すると、正解データ付近に収束していることが分かる。

シアンの楕円：初期値(start)
薄いシアンの楕円：反復過程(trj)
青の楕円：反復終了時(end)

plt.figure()
plt.plot(data[0,:],data[1,:],'y.',label="data")
plt.plot(pts0[0,:],pts0[1,:],'c',label="start")
for i in range(itr):
    x0=X[:,i]
    x0=x0[:,np.newaxis]
    pts,_,_,_=para2elp(x0)
    plt.plot(pts[0,:],pts[1,:],'c',alpha=0.2)

plt.plot(pts[0,:],pts[1,:],'c',label="trj",alpha=0.2)
plt.plot(pts[0,:],pts[1,:],'b',label="end")
plt.plot(data_ref[0,:],data_ref[1,:],'r--',label="ref")
plt.axis("square")
plt.axis("equal")
plt.legend()
plt.xlabel("x")
plt.ylabel("y")
plt.grid()
plt.show()

残差もまんべんなく正解データに近づいている。

plt.figure()
plt.plot(nrm0,'c',label="start")
for i in range(itr):
    x0=X[:,i]
    x0=x0[:,np.newaxis]
    nrm=F(x0)
    plt.plot(nrm,'c',alpha=0.2)
plt.plot(nrm,'c',lw=1,alpha=0.2,label="trj")
plt.plot(nrm,'b',label="end")
plt.plot(nrm_ref.ravel(),'r--',label="ref")
plt.xlabel("data id")
plt.ylabel("MahNorm")
plt.legend()
plt.grid()
plt.show()

反復による残差二乗和の変化は以下。実は30回くらいで収束していた。

###
plt.figure()
plt.plot(sq.ravel(),'.-')
plt.xlabel("itr id")
plt.ylabel("Cost")
plt.grid()
plt.show()