LoginSignup
4
1
@akisame_rage(秋雨 零時)

ストワのレーダーでカルマンフィルタ

Last updated at Posted at 2023-12-08

はじまり

全国のストームワーカーの皆様おはこんばんちわ。
今回は一部界隈で話題に上がるカルマンフィルタを使用してみようという
Stormworks 第1 Advent Calendar 2023　9日目の記事です。
主にノイズの多い（追尾）レーダーや（偵察）レーダー、（射撃管制）レーダーで使うフィルタです。
なおニコニコ動画にも同じ内容のものを投稿していますのでぜひご覧ください。

カルマフィルタとは

誤差のある観測値から、真の状態を推定するアルゴリズムです。
例を挙げてみます。

  • 秒速100mで走ってる
  • 誤差のある速度計を持っている
  • 一秒後、速度計は秒速90mだった

という状況を想定します。
すると前も秒速100mで走ってたら、速度計が秒速90mであっても秒速100mの可能性が高くなり、
一秒前が秒速110mだったら、一秒後は秒速90mの可能性が高くなります。

だいたいこんな感じのアルゴリズムで動くのがカルマンフィルタです。

カルマンフィルタのアルゴリズム

以下のとおりです。1

推定

  1. 推定値を求める
  2. 推定値の共分散行列を求める

更新

  1. 推定値と観測値の差を求める（残差）
  2. カルマンゲインを求める
  3. 残差とカルマンゲインからシステムの状態を決定する
  4. システムの共分散行列を更新する

カルマンフィルタの実装

それじゃ実装します。
何言ってるんだこいつと思うかもしれませんが、フィルタの理論的なことより実装するほうが簡単です。
使い方を覚えてそこから深掘りしたほうが理解が深まる部分もあります。
参考文献のとおりに数式を書くと以下のとおりです。1

\displaylines{
\begin{align}
\bar{x}&=Fx+Bu\\
\bar{P}&=FPF^T+Q\\
y&=z-H\bar{x}\\
K&=\bar{P}H^T(H\bar{P}H^T+R)^{-1}\\
x&=\bar{x}+Ky\\
P&=(I-KH)\bar{P}\\
\end{align}
}

ストームワークスのLuaには行列演算ライブラリがありません。ですので行列演算を実装します。
必要なのは和、差、積、逆行列、転置行列です。

matrix.lua
--Matrix addition
function sum(A, B)
	local r = {}
	for i = 1, #A do
		r[i] = {}
		for j = 1, #A[1] do
			r[i][j] = A[i][j] + B[i][j]
		end
	end
	return r
end

--matrix subtraction
function substruct(A, B)
	local r = {}
	for i = 1, #A do
		r[i] = {}
		for j = 1, #A[1] do
			r[i][j] = A[i][j] - B[i][j]
		end
	end
	return r
end

--Matrix Multiplication
function multiplier(A, B)
	if #A[1] ~= #B then return nil end
	local r = {}
	for i = 1, #A do r[i] = {} end
	for i = 1, #A do
		for j = 1, #B[1] do
			local s = 0
			for k = 1, #B do s = s + A[i][k] * B[k][j] end
			r[i][j] = s
		end
	end
	return r
end

--inverse matrix
function inv(M)
	local n = #M
	local r = {}
	for i = 1, n do
		r[i] = {}
		for j = 1, n do r[i][j] = (i == j) and 1 or 0 end
	end
	for i = 1, n do
		local pv = M[i][i]
		for j = 1, n do
			M[i][j] = M[i][j] / pv
			r[i][j] = r[i][j] / pv
		end
		for k = 1, n do
			if k ~= i then
				local f = M[k][i]
				for j = 1, n do
					M[k][j] = M[k][j] - f * M[i][j]
					r[k][j] = r[k][j] - f * r[i][j]
				end
			end
		end
	end
	return r
end

--transpose matrix
function transpose(M)
	local r = {}
	for i = 1, #M[1] do
		r[i] = {}
		for j = 1, #M do r[i][j] = M[j][i] end
	end
	return r
end

逆行列Invは引数に与えたテーブルの値が変化してしまうので注意が必要です。
つぎにカルマンフィルタの関数を実装します。

kalman.lua

--kalman filter
function KF(X, u, F, P, z, H, Q, R, I)
	local y, K
	--predict
	X = multiplier(F,X)-- multiplier(B,u)

	P = sum(multiplier(multiplier(F,P),transpose(F)),Q)

	--update
	y = substruct(z, multiplier(H,X))

	K = multiplier(multiplier(P, transpose(H)), inv(sum(multiplier(multiplier(H, P), transpose(H)), R)))

	X = sum(X, multiplier(K, y))
	P = multiplier(substruct(I, multiplier(K, H)), P)
	return X, P
end

はいできました。フィルタのコード自体は長くありません。
なお今回は制御入力を抜いてあります。
また、Iは単位行列です。

こんどは変数の意味を把握し、モデル化していきましょう。
プログラムを見ると行列はX、F、u、Z、H、P、Q、Rです。これらは上記のとおりです。
X：状態変数
F：状態遷移行列
u：制御入力
z：観測変数
H：観測関数
P：共分散行列
Q：システムノイズの分散
R：観測ノイズのの分散

これだけじゃわからないので実際にストワのレーダーの距離を使って、目標との距離と速度（視線速度）を求めてみましょう。
距離と速度なので状態変数には、距離と速度が入ります。

\displaylines{
\begin{align}
x=\begin{bmatrix}
distance_{target}\\
speed_{target}
\end{bmatrix}
\end{align}
}

次に状態遷移行列です、ここには運動方程式が入ります。
運動方程式は距離+速度*dtですね。
これを状態遷移関数にするとこうなります。

\displaylines{
\begin{align}
F= \begin{bmatrix} 1&dt\\
0&1 
\end{bmatrix}
\end{align}
}

次に観測変数zですが、センサーの情報は距離だけなので下記のようになります。

\displaylines{
\begin{align}
z=\begin{bmatrix} distance_{radar}
\end{bmatrix}
\end{align}
}

観測関数は状態変数を観測関数に変換する行列です。
Hxがzと同じになればOKです。
なので今回は下記のようになります。

\displaylines{
\begin{align}
H=\begin{bmatrix}1&0 \end{bmatrix}
\end{align}
}

システムノイズの分散は置いといて観測ノイズの分散からやります。
ストワにおいて、距離の誤差は±0.01％の一様分布です。
カルマンフィルタはガウス分布にしか対応していませんが、ガウス分布であると仮定して使用します。
観測ノイズの分散は一様分布の分散、（距離×0.02）^2/12となります。
以上のことからRは以下のようになります。

\displaylines{
\begin{align}
R=\begin{bmatrix} \frac{(dictance_{target}*0.02)^2}{12} 
\end{bmatrix}
\end{align}
}

さて、システムノイズの分散はいくらか？というとぶっちゃけこれがわかりません。
ここの部分はプログラムを実装した上で、実際に試験しながら調節していくことがほとんどだと思われます。
参考文献2から運動方程式の白色ノイズモデルを採用しますと、以下のようになります。

\displaylines{
\begin{align}
Q=\begin{bmatrix} \frac{dt^3}{3}&\frac{dt^2}{2}\\
\frac{dt^2}{2}&dt\\
\end{bmatrix} \phi
\end{align}
}

Φは適当な値を実験しながら探します。
実装して動かしてみましょう。
sclarはスカラー倍する関数、identityは単位行列を作る関数です。
コードでは最初から距離、方向、仰角もカルマンフィルタにかけています。

onTick

pi = math.pi
pi2 = pi * 2
dt = 1 / 60

--Scalar multiplication 
function scalar(a, M)
	local r = {}
	for i = 1, #M do
		r[i] = {}
		for j = 1, #M[1] do
			r[i][j] = M[i][j] * a
		end
	end
	return r
end

--identity matrix
function identity(n)
	local M = {}
	for i = 1, n do
		M[i] = {}
		for j = 1, n do
			if i == j then
				M[i][j] = 1
			else
				M[i][j] = 0
			end
		end
	end
	return M
end

--initilaize
I2 = identity(2)
distance = {{0},{0}}
azimuth = {{0},{0}}
elevation = {{0},{0}}

distanceP = {{100,0},{0,100}}
azimuthP = {{100,0},{0,100}}
elevationP = {{100,0},{0,100}}

--noiseQ
distanceQ = scalar(500, { { dt ^ 3 / 2, dt ^ 2 / 2 }, { dt ^ 2/2, dt } })
bearingQ = scalar(10, { { dt ^ 3 / 2, dt ^ 2 / 2 }, { dt ^ 2/2, dt } })

F={{1, dt},{ 0, 1}}
H = {{1,0}}
isInit = false

function onTick()

	local isContact=false
	if input.getBool(1) then
		radar={r=input.getNumber(1),a=input.getNumber(2)*pi2,e=input.getNumber(3)*pi2}
		isContact=true
	end

	if isContact then
		if not(isInit) then
            --init X 
			distance = {{radar.r},{0}}
			azimuth = {{radar.a},{0}}
			elevation = {{radar.e},{0}}
			isInit = true
		else
			local u,z={},{}
            --distance
			sigmaDistance = (distance[1][1] * 0.02) ^ 2 / 12
			R={{sigmaDistance}}
			z={{radar.r}}
			distance,distanceP= KF(distance, u, F, distanceP, z, H, distanceQ, R, I2)

            --bearing(azimuth and elavation)
			sigmaBearing = (pi2 * 0.002) ^ 2 / 12
			R={{sigmaBearing}}

			z={{radar.a}}
			u = {  }
			azimuth,azimuthP= KF(azimuth, u, F, azimuthP, z, H, bearingQ, R, I2)

			z={{radar.e}}
			u = {  }
			elevation,elevationP= KF(elevation, u, F, elevationP, z, H, bearingQ, R, I2)

            --output
			for i = 1, #distance do
				output.setNumber(i, distance[i][1])
			end
			for i = 1, #azimuth do
				output.setNumber(i+2, azimuth[i][1])
			end
			for i = 1, #elevation do
				output.setNumber(i+4, elevation[i][1])
			end
		end
	else
        -- no contact (reset)
		isInit=false
		distance = {{0},{0}}
		azimuth = {{0},{0}}
		elevation = {{0},{0}}
		
		distanceP = {{100,0},{0,100}}
		azimuthP = {{100,0},{0,100}}
		elevationP = {{100,0},{0,100}}
	end

end

これで実際に動かしてみましょう、ノイズが取れた、思ってたよりノイズが取れない、などの感想があると思います。
カルマンフィルタの利点と欠点をまとめると以下のようになります。

メリット

  • ノイズの軽減
  • 速度や加速度などの各パラメータの同時推定
  • 再帰的なフィルタである（更新はなんどでも回せる）
  • 共分散行列が使える（マハラノビス距離が使えます）

デメリット

  • ガウス分布にしか対応していない
  • 初期値の設定から収束するまで時間がかかる

ノイズを取ることだけがカルマンフィルタではないことがわかりますね。
ノイズを取るならレーダーを二つ搭載し、1チック内に更新を二回回すことでかなり低減できます。

ここまで理解できたら、レーダーの情報をxyzの直行座標系に変換して、それにカルマンフィルタをかければ目標の座標や速度が計算できます……。
だがしかし、実際のところそれはあまりよくありません。

カンマンフイルタで使われる分散は、システムと観測値の分散だからです。
レーダーの分散はわかっていても、その数値を変換したら変換後の分散はわからなくなります。そもそもストワのレーダーの誤差は距離と方向で違います。出来なくはありませんが、精度が落ちてしまいます。

そこで使うのが非線形カルマンフィルタです。
非線形カルマンフィルタには拡張カルマンフィルタ、無香料カルマンフィルタ、粒子フィルタ（厳密に言えばKFではない）があります。
この中で無香料カルマンフィルタは文字数が多く、粒子フィルタは重いのでストワ上では推奨しにくいです。
故に拡張カルマンフィルタがおすすめで、ソースも上記のものを少しいじるだけです。

下記に実際に使用してるモデルを記載しておくので、参考文献3をみて実際にやってみましょう。

\displaylines{
\begin{align}
x&= \begin{bmatrix}
x\\
x_{velocity}\\
y\\
y_{velocity}\\
z\\
z_{velocity}\\

\end{bmatrix}\\
F&=\begin{bmatrix}
1&dt&0&0&0&0\\
0&1&0&0&0&0\\
0&0&1&dt&0&0\\
0&0&0&1&0&0\\
0&0&0&0&1&dt\\
0&0&0&0&0&1\\
\end{bmatrix}\\

h(x)&= \begin{bmatrix}
\sqrt{x^2+y^2+z^2}\\
\sin^{-1}(\frac{z}{\sqrt{x^2+y^2+z^2}})\\
\tan^{-1}(\frac{x}{y})
\end{bmatrix}\\

\frac{\partial h(x)}{\partial x}&=\begin{bmatrix}
\frac{x}{\sqrt{x^2 + y^2 + z^2}}&0&\frac{y}{\sqrt{x^2 + y^2 + z^2}}&0&\frac{z}{\sqrt{x^2 + y^2 + z^2}}&0\\
\frac{-xz}{(x^2 + y^2)^{\frac{3}{2}}(\frac{z^2}{x^2 + y^2} + 1)}&0&
\frac{-yz}{(x^2 + y^2)^{\frac{3}{2}}(\frac{z^2}{x^2 + y^2} + 1)}&0&
\frac{1}{\sqrt{x^2 + y^2}(\frac{z^2}{x^2 + y^2} + 1)}&0\\
\frac{xy}{(x^2 + y^2)^{\frac{3}{2}}\sqrt{\frac{-y^2}{x^2 + y^2} + 1}}&0&
\frac{\frac{y^2}{(x^2 + y^2)^{\frac{3}{2}}} - \frac{1}{\sqrt{x^2 + y^2}}}{\sqrt{\frac{-y^2}{x^2 + y^2} + 1}}&0&0&0\\
\end{bmatrix}

\end{align}
}

なお上記のモデルはY軸が前の右手系です。Z軸が前の場合はPythonなどで出力計算してください。

おわり

参考文献

  1. 6.2 カルマンフィルタのアルゴリズム,Roger R. Labbe,訳inzkyk ↩ ↩2

  2. 7.3 プロセスノイズ行列の設計,Roger R. Labbe,訳inzkyk ↩

  3. 第 11 章 拡張カルマンフィルタ,Roger R. Labbe,訳inzkyk ↩

4
1
0

Register as a new user and use Qiita more conveniently

  1. You get articles that match your needs
  2. You can efficiently read back useful information
  3. You can use dark theme
What you can do with signing up
Sign upLogin
4
1