計測自動制御学会東北支部 第 335 回研究集会 (2021.10.29)
資料番号 335-2

非線形システムに対する状態推定フィルタの設計
Design of State Estimation Filter for Non-linear Systems○田中燎磨 ∗ ，村松鋭一 ∗○ Ryoma Tanaka∗ , Eiichi Muramatsu∗*山形大学*Yamagata University
キーワード : 状態推定 (state estimation)，非線形システム (nonlinear system)，拡張カルマンフィルタ(extended Kalman ﬁlter)，UKF (unscented Kalman ﬁlter)，雑音除去 (noise elimination)

連絡先 : 〒 992-8510 米沢市城南町 4–3–16 山形大学大学院応用生命システム工学専攻
村松鋭一，Tel.: (0238)26-3327，Fax.: (0238)26-3327，E-mail: muramatu@yz.yamagata-u.ac.jp

1.

はじめに

また UKF は EKF では推定できなかった微分不可能な不連続な非線形システムに対しても対1960 年代初頭にカルマンによって提案された応可能である.

カルマンフィルタ (KF) は現代でもなお代表的なシステムの状態推定の方法として様々な分野への応用が期待されている．

本研究では非線形状態推定フィルタである UKFの推定精度を検証し，これをもとに新しく改良した UKF を提案しその有効性について考える．

カルマンフィルタは線形なシステム方程式に対して最適な推定値を求めるものであるが，実際に取り扱う問題の多くは非線形であり汎用性

2.

非線形状態推定フィルタが低いといった問題点がある．

2.1

非線形なモデルの状態方程式

そうしたことなどから近年，非線形システム
本研究で研究対象とするのはつぎの状態方程に対しても適用可能なフィルタとして拡張カルマンフィルタ (EKF) やアンセンテッドカルマン式で表されるシステムである．

フィルタ (UKF) についての研究が進められていx[k + 1] = f (x[k]) + v[k](1)

J. K. Uhlman の二人によって提案された UKF

y[k] = h(x[k]) + w[k](2)はシグマポイントという少数個のサンプル点をここで v[k] と w[k] はシステム雑音と観測雑用いて推定値を求める手法であり拡張カルマン音を表しており，互いに独立である．そして関フィルタ (EKF) と比較すると精度がよい優位性

数 f (・) と h(・) はどちらも非線形関数であるもを持っていることから特に活用が期待されていのとする．

る．その中でも 1990 年代半ばに S. J, Julier とるフィルタである.

–1–

2.2

アンセンテッドカルマンフィルタ (UKF) 算する．
X0 (k − 1) = x̂(k − 1)
(3)√√
Xi (k − 1) = x̂(k − 1) + n + k( P (k − 1))i

アンセンテッドカルマンフィルタ (UKF) は
拡張カルマンフィルタ (EKF) の手法である関数
けた確率変数の統計量を近似的に求める手法と(4)√Xn+i (k − 1) = x̂(k − 1) − n + k( P (k − 1))iなっている．そして U 変換を用いることによっ(5)の線形近似を用いる代わりに、非線形変換を受√

また、重みをつぎのようにおく.て推定精度の向上を図っている．
また UKF は EKF と比べて非線形変換を受け

w0 =た状態の平均ベクトルや共分散行列をより正確に求めることが可能なのでシステムの非線形性が強い場合に有効であり小数のサンプル点を用いて推定を行っていることより計算量が少ない

k1, wi =n+k2(n + k)

(6)

予測ステップは次の式で表される．

Xi− (k) = F (Xi (k − 1))

ここで i = 0, 1, ..., 2n(7)優位性がある.

x̂− (k) =UKF のアルゴリズムについては大きく分け2n∑wi Xi− (k)(8)i=0て以下の２つのステップがある.
・予測ステップ

P − (k) =・フィルタリングステップ∑2n−−i=0 wi (Xi (k) − x̂ (k))(Xi− (k) − x̂− (k))T + σv2 bbT

予測ステップにおいて推定した値と観測した(9)値の誤差をフィルタリングステップにおいてフ

ただし，x̂− (k) は事前状態推定値，P − (k) は事ィードバックすることにより誤差の低減を図っ

前誤差共分散行列である．また，シグマポインている.

トの再計算を x̂0 (k − 1) = x̂(k − 1) で行い，以予測ステップでは１時刻前の推定値 から現時下の計算を行う．

刻での 事前状態推定値を求め，またフィルタリ

X0− (k) = x̂− (k)ングステップでは予測ステップにおいて求めた

Xi− (k) = x̂− (k) −事前状態推定値を最新の観測値を用いて修正し√√n + k( P − (k))i(10)(11)て事後状態推定値を求める流れとなっている.
ここで i=0,1,...,2n−Xn+i(k) = x̂− (k) −√√n + k( P − (k))i (12)

ここで i=0,1,...,2nYi− (k) = h(Xi− (k))
Fig. 1ŷ − (k) =UKF の大まかなアルゴリズム

2n∑wi Yi− (k)(13)
(14)

i=0

従来から知られている UKF のアルゴリズムの詳細については以下の通りである 1) ．

−
Pyy(k) =wi (Yi− (k) − ŷ − (k))2(15)i=0

最初に 1 時刻前に得られた状態推定値 x̂(k−1)

P (k − 1) とし 2n + 1 個のシグマポイントを計

2n∑−Pxy(k) =2n∑wi (Xi− (k) − x̂− (k))(Yi− (k) − ŷ − (k))

i=0(16)–2–ただし，Yi− (k) は出力のシグマポイントの更

この非線形状態方程式に従来の UKF を適用し− (k) は事前出
新,ŷ − (k) は事前出力推定値，Pyy

シミュレーションを行ったところ以下の Fig.2 が− (k) は事前状態・出力誤力誤差共分散行列，Pxy得られた. また平均二乗誤差である RMSE(Root

差共分散行列である．

Mean Squared Error) の値は 5.9676 となった.
Fig.2 において赤線は真値, 青線は UKF によフィルタリングステップは次の式で表される．

g(k) =− (k)Pxy−2Pyy(k) + σwる推定値を表している.

(17)

x̂(k) = x̂− (k) + g(k)(y(k) − ŷ − (k))(18)

−P (k) = P − (k) − g(k)(Pxy(k))T(19)を求める．
ここで g(k) はカルマンゲイン,x̂(k) は状態推定値,P (k) は事後誤差共分散行列である．以上
が UKF のアルゴリズムである.

3.

新しい手法の提案

3.1

従来の UKF

Fig. 2

まず初めに雑音を考慮した対象となる非線形

3.2

UKF を適用した場合の出力結果

改良型 UKF の提案

離散時間システムを考える.
ここで RMSE をより小さくするために改良型

x[k + 1] = f (x[k]) + v[k](20)y[k] = h(x[k]) + w[k](21)

UKF を次のように考える.
本研究では非線形関数 f (・) の非線形性の強さに応じて予測ステップの計算式を切り替える方

ここで v[k] と w[k] はシステム雑音と観測雑法を提案する. 非線形性の強さは f (・) の 2 階微音を表しており互いに独立であり，関数 f (・) と分に x の推定値である x̂ を代入した値の大きさ

h(・) はどちらも非線形関数である.

によって測る. 非線形性が強い場合，予測ステッ

出力 y[k] から x の推定値を逐的に求めていくプに誤差が生じる可能性がある. そこで 2 階微のがアンセンテッドカルマンフィルタ (UKF) で分の値がしきい値 β を超えたら従来の UKF のある.

予測ステップの式を用いずパラメータ α を用い

次に本研究で例として用いる推定対象は非線て予測の更新幅を変える方法を試みる.

形状態方程式で表される次のシステムである.

このシステムの状態方程式 f (・) の２階微分は以下のような式となり Fig.3 が得られる. Fig.3x(k + 1) = f (x(k)) + v(k)(22)
= 0.23x + 9 sin(0.4x) + v(k)から± 1.3 の範囲内で値が変化していることが分かる.

y(k) = h(x(k)) + w(k) = x + 1.5 sin(x) + w(k)d2 f (x)= −1.44 sin(0.4x)
dx2(23)
ここで v(k) は平均 0，分散 1， w(k) は平均 0，分散 3 の正規性白色雑音とする.

(24)

この x に推定値 x̂ を代入した値が ﬁg.3 である.

–3–

次に α=0.5 としたときの場合を考え，シミュレーションを行うと以下のような結果が得られその時の RMSE は 19.0030 となり α=1.0 と設定したときに比べ推定精度が低下することが分かった. Fig.5 においても赤線は真値, 青線は UKF による推定値を表している.

Fig. 3関数 f (・) の 2 階微分に x̂ を代入した値

ﬁg.3 からここで UKF の予測ステップにおけるXi− (k) = F (Xi (k − 1))(25)

２階微分の値が 1.0 を超えた場合は以下を適用してシミュレーションを行う.

Xi− (k) = Xi (k−1)+α(F (Xi (k−1))−Xi (k−1))

Fig. 5
結果

α=0.5 を UKF に適用した時の出力(26)
このとき α を 1.0 とすると (23) 式と (24) 式4.

は一致することが分かる.

シミュレーション結果に基づく考察

α を 1.2 としたときのシミュレーション結果を以下に示す. このときの RMSE は 2.7048 と本研究では α=1.0，α=1.2，α=0.5 とした 3なり従来の手法よりも推定精度が上がったこと

つの場合のシミュレーションを行うことで真値が確認できた.と推定値との推定誤差を検証した.
またそれぞれの数値においてのシミュレーシ

Fig.4 において赤線は真値, 青線は UKF によョンを 10 回行いその RMSE の平均を求めるとる推定値を表している.

それぞれ 4.227，3.689，19.405 となり α=1.2 と設定すると従来の手法よりも推定精度が向上しα=0.5 と設定すると推定制度は低下することが確認できた. 以下にその結果を示す.
以上から非線形関数の 2 階微分の値が大きい
ときはあらかじめ設定したシグマポイントを非線形関数 f (・) により非線形変換したときに誤差が大きくなることがある．そこで α=1.2 と設定した方が誤差が小さくなり推定精度が向上したと考えられる.

Fig. 4
結果

α=1.2 を UKF に適用した時の出力–4–Table 1

case1
case2
case3case4
case5
case6
case7
case8
case9
case10avarage5.

4) 山北昌毅:UKF(Unscented Kalman Filter)って何?Systems, controlandinf ormation, vol.50,
no. 7, pp. 261-266, 2006-07-15.

各パラメータ値における RMSE

α=1.0
5.96764.03103.58615.55354.32383.99004.46293.8553
3.19703.30794.2275α=1.22.70483.32674.13263.57083.98793.74892.87615.2286
3.96753.35153.6896

α = 0.5
19.00316.36320.96318.14518.45823.27317.88321.26018.39520.30519.4055) 山 北 昌 毅:UKF(Unscented Kalman Filter)と KF は ど う 違 う の?ア イ サ イ 問 答 教 室Systems, controlandinf ormationVol.53,No7,pp. 282-283, 20096) S.JulierandJ Uhlmann:Unscented ﬁltering andnonlinear estimatin Proceedings of The JEEEl
VoL 92，No ．3，pp ．401 − 421 （2004）

おわりに本研究では対象となるスカラーの非線形関数の２階微分値を考慮して新しいフィルタを提案
しそのシミュレーションを行うことで推定精度を比較した. そして従来の手法よりも有効であることを確認した.
今後は α を別の値にした場合には出力誤差にどう影響するかだけでなく β の値も考慮しながら各パラメータを変えることで推定精度の向上がどう変化するのかさらなる検討を深めたいと考えている.
また本研究においては様々な値でシミュレーションをしていく中で α や β のパラメータ値を決定したが今後はその値を理論的に求める手法
や UKF のアルゴリズムを改良した少ない計算量で高精度な新しいフィルタについても視野に入れながら研究を進めていきたい.

参考文献
1) 足立修一, 丸田一郎: カルマンフィルタの基礎,
東京電機大学出版, 95/191 (2013)2) 片山徹: 新版応用カルマンフィルタ, 朝倉書店,(2000)
3) 大住晃, 亀山 建太郎, 松田 吉隆: カルマンフィ
ルタとシステムの同定:動的逆問題へのアプロー
チ, 森北出版, (2016)–5–