フーリエ変換~ラプラス変換~Z変換

はじめに

プリズムをかけて赤から紫の順に太陽光のスペクトルを表現させるように、フーリエ級数、フーリエ変換・逆変換、ラプラス変換・逆変換、Z変換・逆変換が、関数の(広義的)周波数特性を解析するとき、必ず登場する。フーリエ変換、ラプラス変換、Z変換の計算式および(広義的)周波数領域におけるイメージ表現の例を以下のように示す。

フーリエ級数

周期2Lの区分的に滑らかな周期関数\(f(t)\)は、不連続点を除いて、$$ f(t) = \frac{1}{2}a_0 + \sum_{n=1}^{\infty} (a_n cos\frac{n\pi}{L}t + b_n sin\frac{n\pi}{L}t) $$ で表される。また実フーリエ級数、フーリエ級数の三角関数表現ともいう。
ただし、\(\small a_0 = \frac{1}{L} \int_{-L}^L f(t)dt , \space a_n = \frac{1}{L} \int_{-L}^L f(t)cos\frac{n\pi}{L}tdt ,\) \( \space b_n = \frac{1}{L} \int_{-L}^L f(t)sin\frac{n\pi}{L}tdt \)

また複素フーリエ級数がフーリエ級数の複素表現ともいう。
$$ f(t) = \sum_{n=1}^{\infty} C_n e^{\frac{in\pi t}{L}} ……①$$ ただし、\(C_n=\frac{2}{L}\int_{-L}^L f(t)e^{-\frac{in\pi t}{L}}dt ……②\)

周期関数\(f(t)\)が偶関数のとき、フーリエ余弦級数となる。
$$ f(t) = \frac{1}{2}a_0 + \sum_{n=1}^{\infty} a_n cos\frac{n\pi}{L}t $$ただし、\(\small a_0 = \frac{2}{L} \int_{0}^L f(t)dt ,\space a_n = \frac{2}{L} \int_{0}^L f(t)cos\frac{n\pi}{L}tdt \)

周期関数\(f(t)\)が奇関数のとき、フーリエ正弦級数となる。
$$ f(t) = \sum_{n=1}^{\infty} b_n sin\frac{n\pi}{L}t $$ただし、\( b_n = \frac{2}{L} \int_{0}^L f(t)sin\frac{n\pi}{L}tdt \)

フーリエ変換・逆変換

\(f(x)\)が非周期関数のとき、周期2L→無限大にすると、フーリエ級数がフーリエ変換(FT: Fourier Transform)・逆変換(IFT: Inverse Fourier Transform)に拡張される。区分的に滑らかで絶対可積分な関数\(f(x)\)について、式②よりフーリエ変換は次の式で表される。\( F(\omega) \)は式②の\( C_n \)に相当する、いわゆる周波数成分となる振幅である。
$$ F(\omega) = \int_{-\infty}^{\infty} f(x)e^{-i\omega x}dx $$
式①よりフーリエ逆変換は次の式で表される。これで\(F(\omega)\)を元の関数\(f(x)\)に戻す。
$$ f(x) = \frac{1}{2\pi} \int_{-\infty}^{\infty} F(\omega)e^{i\omega x}d\omega $$
f(x)が偶関数のとき、フーリエ余弦変換フーリエ余弦逆変換となる。
$$ F(\omega) = 2\int_{0}^{\infty} f(x)cos\omega xdx \\ f(x) = \frac{1}{2\pi}\int_{-\infty}^{\infty} F(\omega)cos\omega xd\omega $$
f(x)が奇関数のとき、フーリエ正弦変換フーリエ正弦逆変換となる。
$$ F(\omega) = -2i\int_{0}^{\infty} f(x)sin\omega xdx \\ f(x) = \frac{i}{2\pi}\int_{-\infty}^{\infty} F(\omega)sin\omega xd\omega $$ フーリエ変換の例として、時間領域のデルタ関数(超関数≠普通の関数) \( \delta(x-0) \) の周波数スペクトルは定数 1 になる。つまり周波数に関わらず 1 だから,全部の周波数成分が等しく含まれている。逆にいうと,あらゆる周波数成分を等しく重ね合わせるとデルタ関数が作り出せる(フーリエ逆変換)。

さらに、時刻\( x_1 \) にインパルスが立っているデルタ関数\( \delta(x-x_1) \)はフーリエ変換すると、\( F(\omega) = e^{-i\omega x_1} \) つまり螺旋 \( cos\omega x_1 – isin\omega x_1 \)となる。以上の定数 1 になるのは\(X_1=0\)のときの特例である。

合成積と関数の積

合成積(畳み込み)$$ f*g(t) = \int_{-\infty}^{\infty} f(x)g(t-x)dx $$
合成積のフーリエ変換 $$ F[f*g(t)] = F[f(x)] F[g(x)] $$
関数の積のフーリエ変換 $$ F[f(x)g(x)] = \frac{1}{2\pi} F[f(x)] F[g(x)] $$

フーリエ変換の性質

線形性(重ね合わせの原理) $$ F[af(x)+bg(x)] = aF(\omega)+bG(\omega) $$
伸縮性 $$ F[f(ax)] = \frac{1}{|a|}F(\frac{\omega}{a}) $$
変数シフト $$ F[f(x+a)] = e^{-ia\omega}F(\omega) \\ F[e^{ia\omega} f(x)]=F(\omega-a) $$
対称性 \( F[f(x)]=F(\omega) \)のとき、$$ F[F(x)]=2\pi f(-\omega) $$
共役性 $$ F(-\omega) = \bar{F}(\omega) $$

微分、積分のフーリエ変換

\(f(x)\)が区分的に滑らかで連続、絶対可積分のとき、
$$ F[f'(x)] =i\omega F[f(x)] \\ F[f^{(n)}(x)] = (iw)^{n}F[f(x)] $$
\(f(x)\)が区分的に滑らかで連続、絶対可積分、かつ\( \int_{-\infty}^{\infty}f(x)dx=0 \)の時
$$ F[\int_{-\infty}^{\infty}f(t)dt]=\frac{1}{i\omega}F[f(x)] $$

パーシバルの等式

$$ \int_{-\infty}^{\infty} |f(t)|^2 dt = \frac{1}{2\pi}\int_{-\infty}^{\infty}|F(\omega)|^2 d\omega $$

離散フーリエ変換・逆変換

サンプリング定理から、ある関数(画像、音声など)をそれのもつ最大の周波数の2倍以上の細かさでサンプリングしておけば、つまりサンプリング間隔ナイキスト間隔以下にすれば、サンプリングされたもの(ディジタル画像,ディジタル音声)から元の関数を再現できる。例として、CDはサンプリング周波数が44.1kHz→22.05kHzまでの音声が記録できる。逆にいうと、録音時にそれ以上の周波数の成分が入らないようにしなければならない。またフーリエ逆変換すれば、理論上サンプリング値から得た周波数成分がサンプリング値に戻る。

アナログ信号からサンプリングしたデジタル信号から、フーリエ変換・逆変換は離散フーリエ変換・逆変換と変身する。離散フーリエ変換(DFT: Discrete Fourier Transform)は次の式で表される。
$$ F_n = \frac{1}{N} \sum_{k=0}^{N-1}f_k e^{-i \frac{2n\pi}{N}k} = \frac{1}{N} \sum_{k=0}^{N-1}f_k \bar{\zeta}_{N}^{nk} \\(n=0,1,2,3,…N-1)$$
離散フーリエ逆変換(IDFT: Inverse Discrete Fourier Transform)は次の式で表される。
$$ f_k = \sum_{n=0}^{N-1} F_n e^{i \frac{2n\pi}{N}k} = \sum_{n=0}^{N-1} F_n \zeta_{N}^{nk} \\ (k=0,1,2,3,…N-1)$$
ただし、\( \zeta_{N}^{nk} = e^{i \frac{2n\pi}{N}k}, \space \bar{\zeta}_{N}^{nk} = e^{-i \frac{2n\pi}{N}k}\)

\( \zeta_{N}^{nk} \)をフーリエ行列\( (\zeta_{N}^{nk}) \)にして、フーリエ行列から周波数成分が求まる。フーリエ行列\(M_N\)の複素共役行列による離散フーリエ変換の表現(周波数成分)は次の式で表される。$$ F_N = \frac{1}{N}\bar{M}_Nf_N $$
フーリエ行列\(M_N\)による離散フーリエ逆変換の表現は次の式で表される。$$ f_N = M_NF_N $$ただし、\( M_N = (\zeta_{N}^{nk}), \space \bar{M}_N = (\bar{\zeta}_{N}^{nk}) \)

またフーリエ行列から周波数成分を解く離散フーリエ変換を高速に実行できるのは高速フーリエ変換(FFT: Fast Fourier Transform)である。FFTで計算すると、離散フーリエ変換での\(\small N(2N-1)\)回の計算が、約\( \small \frac{N}{2}(3log_{2}N-1) \)回の計算まで縮まる。\(\small N=2^{10}=1024 \)ならば、2,096,128回の計算が、約14,848回まで減少する。

これで、周波数帯域のスペクトル、ある周波数成分の調べたり、ある周波数成分のフィルタリング(雑音除去)したり、ある周波数成分の挿入したり、フーリエ変換・逆変換が活用される。

ラプラス変換・逆変換

フーリエ変換の複素数\(i \omega \)から\(s=\sigma+i\omega \)に拡張して、殆どの\(f(t)\)が絶対可積分となって、ラプラス変換は次の式で表される。
$$ F(s) = \int_{-\infty}^{\infty} f(t) e^{-st} dt $$
ラプラス逆変換は次の式で表される。
$$ f(t) = \frac{1}{2\pi i} \int_{\sigma – i\infty}^{\sigma + i\infty} F(s) e^{st} ds $$

Z変換・逆Z変換

ラプラス変換の\(f(t)\)をサンプリングした離散信号を\( x(n), z = e^{\sigma + i\omega T} \)とおくと、Z変換は次の式で表される。
$$ X(z) = \sum_{n=-\infty}^{\infty} x(n) z^{-n} $$
逆Z変換は次の式で表される。
$$ x(k) = \frac{1}{2\pi i} \oint_{c} X(z) z^{k-1} dz $$

(広義的)周波数領域のイメージ表現例

Laplace-Fourier-tranform
ラプラス変換、フーリエ変換のイメージ表現例

Z-tranform
Z変換のイメージ表現例

ただし、\( F(s)、F(z) \)の実際は複素数である。

最後に

共にフランス科学者のフーリエ先生(徒)、ラプラス先生(師)は実の師弟関係だ。減衰振動等微分方程式と代数方程式の橋渡し役、即ち微分方程式の解き方としてのラプラス変換・逆変換が公開されたに対して、波動方程式、熱伝導方程式の解き方として、また完全正規直交関数系である\( \left\{ \frac{1}{\sqrt{2 \pi}},\frac{cos nx}{\sqrt{\pi}},\frac{sin nx}{\sqrt{\pi}} \right\}(n=1,2,3…) \)、つまり実数、正弦関数、余弦関数を組み合わたフーリエ級数から無限次元ベクトル空間が構成可能で、更に条件付き関数を時間領域\(f(t)\)または距離領域\(f(x)\)から周波数領域\( F(\omega)\)の表現に転換可能なフーリエ変換が物事/世界の新たな一面を切り拓いた。やがて離散フーリエ変換に基づいたアナログ~デジタルのサンプリング定理等デジタル信号処理に大いに貢献した。
★Jean Baptiste Joseph Fourier、Baron de、1768年3月21日~1830年5月16日、フランスの数学者・物理学者
★Pierre-Simon Laplace、1749年3月23日~1827年3月5日、フランスの数学者、物理学者、天文学者

参考書籍

フーリエ変換、佐藤敏明氏著、ナツメ社出版
今日から使えるフーリエ変換、三谷政昭氏著、講談社出版

1+

ロボット・ドローン部品お探しなら
ロボット翔・電子部品ストア

ポアソン分布~賢い在庫管理、宝くじまで

ポアソン分布

世の中で、ある事象が一定の頻度(確率)で、ある期間中にいつでも起きる可能性があり、ただし正確な発生時刻が知ることができない。例えば、自動車が交差点を通過、赤ちゃんが病院で生まれ、スーパーで商品が販売され、機械が故障との事象がある。機械は月に1回故障、スーパーは1日に牛乳50本を販売、病院に1時間に3人の赤ちゃんが誕生、15分の間に40台の車が交差点を通過したという具体的事象に対して、特定の期間における独立した事象の発生数の確率分布を求めるには、以下のポアソン分布が使用可能である。
$$P[X(t)=k]=\frac{(λt)^k e^{-λt}}{k!}$$
ただし、tは連続時間の長さ、λは単位時間に事象発生の期待値、eは自然対数の底(ネイピア数)。事象が期間t内に発生数がkの確率を計算する。ポアソン分布の期待値、分散ともλである。

在庫管理、ナンバーズ予測への応用例

【在庫管理の使用例】某ECサイトでは、1日に平均でPCを2台販売している。在庫切れの確率を5%以下に抑えたいので、どれだけの量の在庫が必要なのか。
1日にPCを0台販売の確率:
$$P[X(1)=0]=\frac{(2*1)^0 e^{-2*1}}{0!}=e^{-2}$$
1日にPCを1台販売の確率:
$$P[X(1)=1]=\frac{(2*1)^1 e^{-2*1}}{1!}=2e^{-2}$$
1日にPCを2台販売の確率:
$$P[X(1)=2]=\frac{(2*1)^2 e^{-2*1}}{2!}=2e^{-2}$$
1日にPCを3台販売の確率:
$$P[X(1)=3]=\frac{(2*1)^3 e^{-2*1}}{3!}=\frac{4}{3}e^{-2}$$
1日にPCを4台販売の確率:
$$P[X(1)=4]=\frac{(2*1)^4 e^{-2*1}}{4!}=\frac{2}{3}e^{-2}$$
1日にPCを5台販売の確率:
$$P[X(1)=5]=\frac{(2*1)^5 e^{-2*1}}{5!}=\frac{4}{15}e^{-2}$$
なので、1日にPCを3台以上販売の確率:
$$ P[X(1)>2]=1-\sum_{i=0}^{i=2}P[X(1)=i]\\\scriptsize=1-e^{-2}-2e^{-2}-2e^{-2}=0.3233$$
1日にPCを4台以上販売の確率:
$$P[X(1)>3]=1-\sum_{i=0}^{i=3}P[X(1)=i]\\\scriptsize=1-e^{-2}-2e^{-2}-2e^{-2}-\frac{4}{3}e^{-2}=0.1429$$
1日にPCを5台以上販売の確率:
$$P[X(1)>4]=1-\sum_{i=0}^{i=4}P[X(1)=i]\\\scriptsize=1-e^{-2}-2e^{-2}-2e^{-2}-\frac{4}{3}e^{-2}-\frac{2}{3}e^{-2}=0.0527$$
1日にPCを6台以上販売の確率:
$$P[X(1)>5]=1-\sum_{i=0}^{i=5}P[X(1)=i]\\\scriptsize=1-e^{-2}-2e^{-2}-2e^{-2}-\frac{4}{3}e^{-2}-\frac{2}{3}e^{-2}-\frac{4}{15}e^{-2}=0.0166$$
つまり、1日にPCを6台以上販売の確率が1.66%なので、PC 5台以上(最低5台)の在庫が確保できれば、在庫切れの確率を5%以下に抑えられる。

【ナンバーズ予測の使用例】
0~9の数字の出た回数を1000回分もしくは2000回分で平均して、λにしておく。最近の9回、0~9の出た回数+今度も出る(+1)確率を、大きさが大から小の順で並べて、確率が大きい数字が予測の結果とする。注意!:あくまで確率論からの予測(可能性)なので、ポアソン分布から宝くじが当たる意味ではない。

【発明者が残した言葉】
「私がランダム現象を記述する1つの確率分布を確立した」 by フランス数学者・物理学者ポアソン。

1+

ロボット・ドローン部品お探しなら
ロボット翔・電子部品ストア

粒子フィルタ

はじめに

ガウス分布に従う時系列確率変数の非線形関数は必ずしもガウス分布にならない、また非ガウス分布の確率変数が存在するから、優雅なるカルマンフィルタファミリーには限界あり、無理な場合がある。非線形、非ガウス時系列空間モデルに対して、1990年代から未知の状態密度関数p(x)に対して、既知の密度関数q(x)、p(x)⊂q(x) から逐次モンテカルロ法(逐次重要性サンプリングSequential Importance Samplingに、リサンプリングResampling)による粒子フィルタ(パーティクル・フィルタ、PF)が提案されて以来、研究・応用が活発になっている。粒子フィルタがROSのSLAM Gmappingアルゴリズムに組まれている。

粒子フィルタ

状態空間モデル

・システムモデル(状態方程式)
$$x_t = f_t(x_{t-1},v_t)$$

・観測モデル(観測方程式)
$$y_t = h_t(x_t,w_t)$$

ただし、\( v_t、w_t\)はフィルタリングをかける事前に既知で、非ガウス分布可能、\( y_t\)は2項分布またポアソン分布に従うcount dataのモデル化なども可能になる。また、粒子フィルタには、\( v_t、w_t\)はガウス分布でも構わない。
以下に粒子\( x_t^{[i]} \)~\( f_t (x_t^{[i]} | x_{t−1}^{[i]}, v_t) \)として、粒子フィルタリングの手順を示す。

粒子フィルタリングの手順

1、初期化\( (t=0) \)
初期分布\( f_t (x_0^{[i]}, v_0) \)に従って、n個の粒子\( \{x^{[i]}_0 | i = 1,2, ⋯ ,n\} \)を無作為に発生させる。

2、一期先予測\( (t=t+1) \)
粒子\( x_t^{[i]} \)を\( f_t (x_t^{[i]} | x_{t−1}^{[i]}, v_t) \)に従って状態推移させ、それぞれの類似値が\(x_t^{[i]}\)である粒子の集合\( \{\hat{x}^{[i]}_t | i = 1,2, ⋯ ,n\} \)を発生させる。

3、フィルタリング
・重み計算
粒子\( \hat{x}_t^{[i]} \) の重み\( w_t^{[i]} = p(y_k | \hat{x}_k^{[i]}) \)を計算する。
ただし、\( p(y_k | \hat{x}_k^{[i]}) \)は、尤度であり、観測値または類似値ではないことに注意する。

・重みの正規化
$$\hat{w}_t^{[i]} = \frac{w_t^{[i]}}{ \sum_{i=1}^n w_t^{[i]} }$$

・リサンプリング
粒子\( \hat{x}^{[i]}_0 \)を\(\hat{w}_t^{[i]}\)に従った確率でリサンプリングし、粒子集合\( \{x^{[i]}_t | i = 1,2, ⋯ ,n\} \)を発生させる。

・2に戻る

参考文献

粒子フィルタ、著者:樋口知之 先生

1+

ロボット・ドローン部品お探しなら
ロボット翔・電子部品ストア

SLAM~Unscentedカルマンフィルタ

はじめに

UKF(Unscented kalman filter)では無損変換(Unscented Transform)の線形化手法を利用し、つまり(2*n次元+1)個Sigma Pointの線形回帰に基づき、確率変数の非線形関数を線形化する。確率変数の拡張を考慮するので、この線形化は拡張カルマンフィルタEKFで使用するテイラー級数線形化より比較的正確に状態推定できる他、EKFと違い、状態・観測ヤコビアン行列を求める必要でなく、但しEKFと同様にUKFも予測と更新の手順から時系列の状態推定を行う。状態変数、観測変数ともガウス分布に当てるのがカルマンフィルタKFの共通特徴という。またガウス分布に当てないパーティクル(和文粒子、英文Particle)フィルタPFがある。そもそもガウス分布に従う確率変数の非線形関数はガウス分布にならないから、EKF、UKFよりPFの方が精度よく、ROSのSLAM Gmappingアルゴリズムに組まれている。

Unscented Kalman Filter

状態空間モデル

状態モデル(状態方程式)
$$x_t = F \cdot x_{t-1} + B \cdot u_{t-1} + q_{t-1} \\\scriptsize=\begin{bmatrix}1&0&0&0\\0&1&0&0\\0&0&1&0\\0&0&0&0\end{bmatrix}\begin{bmatrix}x_{t-1}\\y_{t-1}\\ψ_{t-1}\\v_{t-1}\end{bmatrix}+\begin{bmatrix}Δt\cdot cosψ_{t-1}&0\\Δt\cdot sinψ_{t-1}&0\\0&Δt\\1&0\end{bmatrix}\begin{bmatrix}v_{t-1}\\ψ_{t-1}\end{bmatrix}+q_{t-1}$$

観測モデル(観測方程式)
$$z_t = H \cdot x_t + r_t \\=\begin{bmatrix}1&0&0&0\\0&1&0&0\end{bmatrix}\begin{bmatrix}x_t\\y_t\\ψ_t\\v_t\end{bmatrix}+r_t$$

予測ステップ

$$χ_{t-1}=(x_{t-1} , x_{t-1}+γ\sqrt{{\sum}_{t-1}} , x_{t-1}-γ\sqrt{{\sum}_{t-1}})\\
\bar{χ}_t^{*}=g(u_t,χ_{t-1})\\
\bar{x}_t=\sum_{i=0}^{2n} w_m^{[i]} \bar{χ}_t^{*[i]}\\
\bar{\sum}_t=\sum_{i=0}^{2n} w_c^{[i]}(\bar{χ}_t^{*[i]}-\bar{x}_t)(\bar{χ}_t^{*[i]}-\bar{x}_t)^T +R_t\\
\bar{χ}_{t}=(x_{t} , x_{t}+γ\sqrt{{\sum}_{t}} , x_{t}-γ\sqrt{{\sum}_{t}})\\
\bar{Z}_{t}=h(\bar{χ}_{t})\\
\hat{z}_{t}=\sum_{i=0}^{2n} w_m^{[i]} \bar{Z}_{t}^{[i]}\\
S_t=\sum_{i=0}^{2n} w_c^{[i]}(\bar{Z}_t^{[i]}-\hat{z}_t)(\bar{Z}_t^{[i]}-\hat{z}_t)^T +Q_t\\
\bar{\sum}^{x,z}_t=\sum_{i=0}^{2n} w_c^{[i]}(\bar{χ}_t^{[i]}-\bar{x}_t)(\bar{Z}_t^{[i]}-\hat{z}_t)^T
$$

更新ステップ

$$K_t=\bar{\sum}^{x,z}_t S^{-1}_t\\
x_t = \bar{x}_t+K_t(z_t-\hat{z}_t)\\
{\sum}_t = \bar{\sum}_t-K_t S_t K^T_t$$

参考文献

Probabilistic Robotics/Sebastian Thrun, Wolfram Burgard and Dieter Fox.

1+

ロボット・ドローン部品お探しなら
ロボット翔・電子部品ストア

SLAM~拡張カルマンフィルタ

概要

SLAM(ロボットの自己位置推定、マッピング同時に行うこと)に使用可能なセンサは、色々とあるが、それぞれ単一のセンサでは完璧ではないため、複数のセンサ情報を、統計的・確率的に組み合わせて、より精密で安定した自己位置を得る手法が一般的だ。状態値(位置、姿勢またはポーズ)、観測値とも正規分布(ガウス分布)に近似可能の場合、カルマンフィルタKFが適用される。また、非線形問題に対して、拡張カルマンフィルタEKFが適用される。勿論、UKF他のフィルタが存在する。なぜかROSに公式に採用されたのはEKFのようだ。ここでは、タイヤパルスセンサ(ホール式や、光学式エンコーダ)からのodom情報と距離センサ(レーザや、カメラなど)からの位置情報を組み合わせて、マッピング地図がある前提でロボットの自己位置、姿勢に拡張カルマンフィルタをかけて推定してみる。

状態空間モデル

$$x_t = f_{t-1}(x_{t-1}^{true}+p_{t-1}) + q_{t-1}\\=\scriptsize\begin{bmatrix}x_{t-1}+v_{t-1}\cdot Δt\cdot cosψ_{t-1} & 0 & 0 \\0 & y_{t-1}+v_{t-1}\cdot Δt\cdot sinψ_{t-1} & 0\\0 & 0 & ψ_{t-1}+ω_{t-1}\cdot Δt\end{bmatrix}+q_{t-1}$$
$$y_t = H_t(x_t^{true}) + r_t\\=\begin{bmatrix}x_t\\y_t\end{bmatrix}+ r_t$$

拡張カルマンフィルタの適用

予測ステップ

事前状態推定値

$$\bar{x}_t=\scriptsize\begin{bmatrix}\hat{x}_{t-1}+v_{t-1}\cdot Δt\cdot cos\hat{ψ}_{t-1} & 0 & 0 \\0 & \hat{y}_{t-1}+v_{t-1}\cdot Δt\cdot sin\hat{ψ}_{t-1} & 0\\0 & 0 & \hat{ψ}_{t-1}+ω_{t-1}\cdot Δt\end{bmatrix}$$

線形化近似(ヤコビアン行列)

$$\hat{F}_t =\frac{\partial f_t(x)}{\partial x}|_{x=\hat{x}_{t-1}}\\=\begin{bmatrix}1 & 0 & -v_{t-1}\cdot Δt\cdot sin\hat{ψ}_{t-1} \\0 & 1 & v_{t-1}\cdot Δt\cdot cos\hat{ψ}_{t-1}\\0 & 0 & 1\end{bmatrix}$$

事前誤差共分散行列

$$\bar{P_t} = \hat{F}_{t-1}\hat{P}_{t-1}\hat{F}_{t-1}^T + Q_{t-1}$$

フィルタリングステップ

カルマンゲイン行列

$$K_t = \bar{P_t}H_t^T(H_t\bar{P_t}H_t^T + R_t)^{-1}$$

状態推定値

$$\hat{x_t} = \bar{x_t} + K_t(y_t – H_t\bar{x_t})$$

事後誤差共分散行列

$$\hat{P_t} = (I-K_tH_t)\bar{P_t}$$

関連記事

研究開発用 台車型ロボット キット

2+

ロボット・ドローン部品お探しなら
ロボット翔・電子部品ストア