カルマンフィルタを使ったノイズ除去

で，現在行き詰まり中のロボット作成ですが，何とかステレオカメラを使ってマップを作らないといけないのでノイズの除去をする方法を検討してます．
解こうとしている問題は，，，

１．ステレオカメラで取得した視差情報を３次元の点群に変換．
２．３次元の点群を地面に投影して，地面に切ったグリッドの高さを求める．
３．地面に切ったグリッドの高さ，もしくは地面の傾きが一定以上大きい箇所は通行不可能なエリアとしてマーキングする．

で，結果的にはマップを「通行可能」「通行不可能」の二通りの値に分類して，これをマップとして生成します．
このためには，地面に切ったグリッドのセル高さを求めないといけないのですが，ステレオの視差情報はノイズが大きく，これをどうやって扱えばいいものかとずっと悩んでました．

上図はカメラと地面に切ったグリッドの簡易図ですが，青の三角形がカメラでその前のマス目が地面に定義したグリッドです．ステレオの視差情報から生成したポイントクラウドは上記のセルのどこかに属する形になりますが，このポイントクラウド一つ一つの高さ情報を使ってマス目一つの高さ情報を定義します．（マス目一つ一つが高さ情報を持っているイメージです．）

ただ，ステレオの視差情報はノイズによる影響が大きいので，同じマス目なのに高さが１．５ｍの時もあれば，２．５メートルとなってしまう時もあります．今回は，このノイズを取るために１次元のカルマンフィルタを使ってみることにしました．そんなにカルマンフィルタには詳しくなかったのでGoogle先生にいろいろと教えてもらって勉強しました．一番わかりやすくまとめられてたのが，下記のサイトです．

https://logics-of-blue.com/kalman-filter-concept/

で，上記サイトのサンプルプログラムを参考に，自分の問題にあてはめて Python コードを作ってみました．
問題設定は，，，，
1.ロボットが前方に動いて，新たなセルが視界に入ってくる．新たなセルの高さの真値は１mとする．
2．新たなセルであるため，当然高さ情報はなし．つまり初期値を 0m としてはじめる．
３．ノイズがいっぱい乗っているステレオの情報を繰り返し使って，真値の１mに近づける．

という問題設定です．ちなみに，カルマンフィルタを選択した時点でステレオノイズに含まれている誤差が平均値０の白色ノイズという前提になってしまうのですが，今回取り除きたいノイズがこのモデルに従うかどうかはわかりません．．．

下記 Python のサンプルコードです．

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

def Kalman(y, xK1, pK1, sigmaW, sigmaV):

    # Predict State Variable
    xK = xK1

    # Update Sigma Variable
    pK = pK1 + sigmaW

    # Update Kalman Gain
    kGain = pK / (pK + sigmaV)

    # Filter State and Variable
    xFilt = xK + kGain * (y - xK)
    pFilt = (1 - kGain) * pK

    return xFilt, pFilt


def CreateFilteredData(measData, initPredVar, sigmaW, sigmaV):

    length = len(measData)
    filteredMeasData = np.zeros(length + 1)
    filteredPredVar = np.zeros(length + 1)
    filteredPredVar[0] = initPredVar

    for i in range(0, length):
        xFilt, pFilt = Kalman(measData[i], filteredMeasData[i], filteredPredVar[i], sigmaW, sigmaV)
        filteredMeasData[i + 1] = xFilt
        filteredPredVar[i + 1] = pFilt

    return np.delete(filteredMeasData, 0), np.delete(filteredPredVar, 0)

def CreateSampleData(height, var, size):

    indexes = [idx for idx in range(0, size)]
    heightListWithNoise = np.random.normal(height, var, size)

    return indexes, heightListWithNoise

if __name__ == '__main__':
    print("Sample Kalman Filter started.")

    height = 1.0
    var = 0.5
    size = 100

    indexes, data = CreateSampleData(height, var, size)
    filteredData, filteredVars = CreateFilteredData(data, 10000, 100, 10000)

    plt.plot(indexes, data)
    plt.plot(indexes, filteredData)
    #plt.plot(indexes, filteredVars)
    plt.show()

上記コードを実行した結果ですが，下記のようになりました．

縦軸がステレオの視差ベースで求めた高さのイメージです．横軸は単純にサイクルカウントです．図を見てわかるように，青の値が純粋な入力値であり，ノイズの影響でだいぶブレブレになってますが，フィルタをかけたあとの緑の値は 1m 付近をあるていどキープしているように見えます．ということで，取りあえずカルマンフィルタを使って高さ方向のノイズ除去をし，その値を用いてマップ作成をしてみます！結果はできれば明日にアップします！