※上記の広告は60日以上更新のないWIKIに表示されています。更新することで広告が下部へ移動します。

  • カルマン・フィルタの特徴
    • 信号は非定常でも可。ただし、エルゴード性を仮定
    • 観測雑音を白色雑音と仮定。
  • 連続系のカルマン・フィルタの理論
x(t):システムを構成する状態変数
u(t):入力される制御変数
y(t):システムから出力され、観測の対象となる観測変数
w(t):観測雑音、白色雑音を仮定 \ E\{ w(t_1),w(t_2)\} = \sigma^2 \delta(t_1 - t_2)
F:システムの状態の変化を表現する関数
H:状態変数x(t)から観測変数y(t)への変換を表現する関数
状態方程式
\frac{dx(t)}{dt} = F(x,u,t)
観測方程式
y(t) = H(x,t) + w(t)
A(t) = \frac{\partial F}{\partial x}, \ B(t) = \frac{\partial F}{\partial u}, \ C(t) = \frac{\partial H}{\partial x}
とおくと、
\frac{dx(t)}{dt} = A(t)x(t) + B(t)u(t)
y(t) = C(t)x(t) + w(t)
となる。
  • 離散系のカルマンフィルタの理論
x_k:状態ベクトル
y_k:観測ベクトル
u_k:制御ベクトル
v_k:状態雑音ベクトル
w_k:観測雑音ベクトル
\Phi_{k+1,k}:時点kからk+1への状態遷移行列
C_{k+1,k}:時点kからk+1への制御行列
H_k:観測行列
V_k:状態雑音v_kの共分散行列
W_k:観測雑音w_kの共分散行列
状態方程式
x_{k+1} = \Phi_{k+1, k}x_k + C_{k+1,k} u_k + v_k
観測方程式
y_k = H_k x_k + w_k
ここで、状態ベクトルx_kは、制御ベクトルu_kからドライブされるものと考え、u_k = D_k x_k^*とする。つまり、状態ベクトルの推定値の変換として制御ベクトルが得られると考える。
また、状態雑音v_kの影響を受けない状態での状態ベクトルの値を状態ベクトルの予測値と定義すると、
x_{k+1,k} = (\Phi_{k+1,k} + C_{k+1,k}D_k)x_k^*
\Psi_{k+1,k} = \Psi_{k+1,k} + C_{k+1,k}D_k
このとき、予測のアルゴリズムは以下のようになる。
  • step1:初期化
k=0,x_k^*(状態推定ベクトル),x_{k,k-1}(状態予測ベクトル),P_{k,k-1}の初期化
  • step2:観測値入力
k=k+1,y_kの入力
  • step3:予測値の計算
x_{k,k-1} = \Psi_{k,k-1}x_{k-1}^*
y_{k,k-1} = H_k x_{k,k-1}
  • step4:カルマンゲインの計算
B_k = P_{k,k-1} H_k^t[H_k P_{k,k-1}H_k^t + W_k]^{-1}
  • step5:推定値誤差共分散の計算
P_{k,k} = [P_{k,k-1}^{-1} + H_k^tW_k^{-1}H_k]^{-1}
  • step6:推定値の計算
x_k^* = x_{k,k-1} + B_k[y_k - H_k x_{k,k-1}]
y_k^* = H_k x_k^*
  • step7:予測値誤差共分散の計算
P_{k,k-1} = \Phi_{k,k-1}P_{k,k-1}\Phi_{k,k-1}^t + V_{k-1}
→step2
添付ファイル