scipy.optimize.minimize を使ったGPS測位計算みたいな最適化計算の動作確認

16270 ワード
測位 scipy GPS optimize Python3 scipy テキストリンク

ここでは、ＧＰＳなどの測位計算の基礎となっている計算を、scipy のOptimizer(最適化の計算を行うライブラリ) を使って計算してみます。

背景

おじさんは今まで、自分で目的関数を設定すると、それを解くための計算式は紙と鉛筆でしこしこ計算して導出していました。しかし昨今は、深層学習もそうであるように、計算式を自分で設定しなくても、データを与えたり、目的関数を与えるだけで最適化の計算をしてくれるライブラリが多く利用できます。ここでは、目的関数（誤差の関数）がわかっている場合の最適化を扱います。

実際に、ここでは目的関数objective を定義して、


def objective(x):
    ...
    return val

これを最小化する計算は、初期条件x0を与えれば、以下の一行で行えます。

from scipy.optimize import minimize
result = minimize(objective, x0=x0, constraints=())

非線形とは？

「非線形(non-linear)」という言葉は意味が分かりにくく、難しそうに聞こえるかもしれません。コンテクストによりますが、ここでは線形である観測は以下のように、もとの値$x$を$h$倍して観測できるものを指します。

y = hx

これでないものが非線形です。今回扱うのは、距離を観測する場合です。$s_0$からの距離が$y$である、という観測は

y = h(x) = |x-s_0|

と書けます。これはx が２倍になってもy は２倍になりません。

問題設定

N個のGPS衛星から発せられた電波をある時刻に受信機が受信したとします。このとき、受信機は衛星からの距離の情報を得ることができます。ただし、センサーバイアス(GPSでは時計誤差) $b$ が含まれるので、これには一定の誤差$\epsilon$が加わっています。

y_n = || \mathbf{p} - \mathbf{s}_N || + b + \epsilon

そこで、誤差を最小にする位置を求めます。同時に、センサーのバイアス b も求めます。なので、位置の３パラメータとバイアスを合わせて、４変数についての最小化問題を考えます。

通常はNewton-Rapson法で解ける問題ですが、ここではそのような導出をせずに行います。

シミュレーション用のデータ作成

ここでは、センサーの位置とユーザの真の位置が与えられたとき、センサーのノイズとバイアスを与えて、以下ように測距信号を生成します。

def generate_obs(usr_pos:list, bias:float, src_poss:dict, sig:float = 1.0):
    """
    シミュレーションで使用する観測値と観測点(電波発信の位置)を生成する。
    センサまでの距離を与えるが、センサーの位置はランダムに設定する。
    Generate range observation and emission device positions.

    n-th mearuement is, 
    y[n] = | p - s[n] | + b + e,    E[e] = 0.0, V[e] = sig^2

    Input
    -----
    usr_pos:list, ユーザの真の位置 user position (3D position)
    bias:float, 観測値に共通に加わるバイアス, sensor bias common to all measurement
    dist:list,  ユーザと電波発信源からの距離、センサーの個数分だけある
    sig:float, センサのノイズ
    """
    sensor_meas = {}
    for lb, s_pos in src_poss.items():
        sensor_meas[lb] = np.linalg.norm(np.array(usr_pos) - np.array(s_pos)) + bias + sig * np.random.randn()
    return sensor_meas

測位計算の実装

目的関数を実装してminimize の引数に入れるだけなのですが、よく見るTutorial とは異なる点があります。それは、目的関数を計算するのに、引数以外のパラメータが必要である点です。ここでは、測距信号とセンサーの位置が必要です。

これらのパラメータをglobal 変数にして関数から参照する方法でも問題なく動作します。よりきれいに書くために、ここでは関数そのもの代わりにcallable なclassを用います。

class ErrorFunction(object):
    """
    測位計算で使用する最小化したい目的関数を定義します。
    与えられた測距信号とセンサーの位置（疑似距離と衛星位置に相当）に対して、
    誤差を最小化するユーザの位置を求めるのが目的です。
　　"""

    def __init__(self, **kwargs):
        """
        目的関数を計算するために必要なパラメータを設定します。
        In this initialization process, parameters required to calculate objective function is set.

        Input
        -----
        **sensor_range:dict, 全センサー(衛星)からの測距信号の値
        **sensor_position:dict, 全センサー(衛星)の位置

        key はセンサーの名前です。
        """
        self.sensor_meas = kwargs["sensor_range"]
        self.src_poss = kwargs["sensor_position"]

    def __call__(self, x):
        """
        最小化したい目的関数。ここではモデルと観測の距離の誤差の二乗和。
        Objective function we want to minimize. Square error of model and observation ran    ge.

        f(x) = sum_n ||  y[n] - (|| p[n] - r[n] || + c) |    |^2

        Input    
        -----
        x:list, 目的関数の引数 (argument of objective functi    on)

        Return    
        ------
        fx:float, 目的関数の値
        """
        cost_func = 0.0
        for k, rho in self.sensor_meas.items():
            if k not in self.src_poss.keys():
                continue
            err = np.array(x[0:3]) - np.array(src_poss[k])
            dist = np.linalg.norm(err) + x[3]
            res = rho - dist
            cost_func = cost_func + res **2
        return cost_func

これを用いて、最小化は測距信号とセンサー位置がそれぞれ与えられているので、

x_init = np.zeros(4)
res = minimize(ErrorFunction(sensor_range=y, sensor_position=s), x0=x_init, constraints=(), tol=1e-6)

のようにして計算できます。

実行結果

以下のコードで動作確認します。

import numpy as np
from scipy.optimize import minimize

usr_pos = [-111.0, 222.0, -333.0]
src_poss = {"S1": [ 100.0,   0.0,    0.0], 
            "S2": [   0.0, 200.0,   0.0], 
            "S3": [   0.0,   0.0, 300.0],
            "S4": [ -20.0, -10.0,   5.0]}
bias = 2.3
sensor_meas = generate_obs(usr_pos, bias, src_poss, sig= 1.0E-3)
res = minimize(ErrorFunction(sensor_range=sensor_meas, sensor_position=src_poss), x0=np.zeros(4), constraints=(), tol=1e-6)

print( "Solver returns {}. x = {}".format(res.success, res.x))
print( "Ground Truth: position={} bias={}".format(usr_pos, bias)  )
print( "Error={}".format(np.linalg.norm(np.array(usr_pos + [bias]) - np.array(res.x))))

結果は以下のとおりです。動いた感じがします。^^;)

Solver returns True. x = [-110.98835806  221.98764992 -332.96362384    2.34037554]
Ground Truth: position=[-111.0, 222.0, -333.0] bias=2.3
Error=0.056933891364635274

まだまだです

最適化計算を使う、という観点からは、手法を選んでJacobian や Hessian を導出して、関数として実装して使用することもできます。実際に解析的に計算できる場合は、それらを利用したほうが計算はロバストになるような気がします。

とりあえず今回はここまで。(2020/03/17)