暴れる値をおとなしくさせるには、カルマンフィルターが便利で高速

センサーの値などが、揺れて安定せずに、困ることがあります。
そういうときは、スカラー値に対するカルマンフィルターが、高速で便利です。

加速度センサーやジャイロスコープの生の値はノイズ込みで揺れまくります。それをおとなしくさせて傾向だけを取り出すのに使えました。また、「OpenCVやDlibの使い方で失敗した諸点」に書いたように、DlibのFace Landmarkの特徴点位置が揺れるのをおとなしくさせるのに使えました。

ただの移動平均ですと、異常値に左右されます。異常値に左右されにくくするために、移動平均のスパンを大きくとると、今度は反応が遅くなります。カルマンフィルターですと、反応の遅さを抑えながら異常値に左右されにくい落としどころを見つけられます。

以下にPythonでclass Kalmanのコードを載せておきます。最初、Kalmanインスタンスを作るときに、パラメータを与えて、以降、観測値をguessメソッドに与え続けるだけです。最初のパラメータの値によって、ずいぶん挙動が変わります。そのためテストデータで、入力の観測値と出力の推定値をPlotして観察し、試行錯誤して適当なところにパラメータ値を設定する作業が必要となります。

class Kalman():
    #
    # Reference: http://www.cs.unc.edu/~welch/media/pdf/kalman_intro.pdf
    #

    def __init__(self, procvar, measvar):
        self.ctr = 0
        self.Q = procvar  # process variance
        self.P_k_minus_1 = 0.0  # a posteri error estimate
        self.R = measvar  # estimate of measurement variance, change to see effect

    def guess(self, input):
        DEBUG = False
        if self.ctr == 0:
            self.xhat_k_minus_1 = input  # a posteri estimate of x
            self.K = 1.0  # Kalman gain
            self.ctr = self.ctr + 1
            return input
        else:
            # time update
            xhat_k = self.xhat_k_minus_1 # a prior estimate of x, transition matrix is identity, no control
            Phat_k = self.P_k_minus_1 + self.Q # a prior estimate of error
            # measurement update
            self.K = Phat_k / (Phat_k + self.R) # Kalman gain
            estimate = xhat_k + self.K * (input - xhat_k) # a posteri estimate of x
            self.xhat_k_minus_1 = estimate # a posteri estimate of x
            self.P_k_minus_1 = (1 - self.K) * Phat_k # a posteri error estimate
            # error variance and kalman gain should become stable soon
            if DEBUG: print("Kalman:","Input",input,"Estimate",int(estimate), "ErrorVar {:.2}".format(self.P_k_minus_1), "KalmanGain {:.2}".format(self.K))
            return estimate