前回 : カルマンフィルタの基礎第7回 - ma-hi-ro Hobby

カルマンフィルタの基礎第8回

ここまでやってきた線形カルマンフィルタでは、
逐次計算によって、カルマンゲイン $G(k)$ と共分散行列 $P(k)$ を求めながら、状態推定を行っていた
今回は、事前に計算することによって、 $G(k)$ と $P(k)$ の収束値を求める方法を紹介する

カルマンフィルタの基礎第8回

定常カルマンフィルタ

これまでは、以下の状態空間モデルに対して、以下のカルマンフィルタのアルゴリズムに応じて逐次計算してきた

$\displaystyle \begin{aligned} x(k+1) &= A x(k) + B \nu(k) \\ y(k) &= C^T x(k) + \omega(k) \\ \end{aligned}$

$\displaystyle \begin{aligned} \hat{x}^-(k) &= A \hat{x}(k-1) \\ P^-(k) &= A P(k-1) A^T + B Q B^T \\ \end{aligned}$

$\displaystyle \begin{aligned} G(k) &= P^-(k) C (C^T P^-(k) C + R)^{^-1} \\ \hat{x}(k) &= \hat{x}^-(k) + G(k) (y(k) - C^T \hat{x}^-(k)) \\ P(k) &= (I - G(k) C^T) P^-(k) \\ \end{aligned}$

今回は、この逐次計算が収束する場合を考える

カルマンフィルタの収束値

事前共分散行列 $P^-(k) \rightarrow P^*$ と仮定すると

$\displaystyle \begin{aligned} G(k) &\rightarrow& G^* &= P^* C (C^T P^* C + R)^{^-1} \\ P(k) &\rightarrow& P^+ &= (I - G^* C^T) P^* \\ P^-(k+1) &\rightarrow& P^* &= A P^+ A^T + B Q B^T \\ \end{aligned}$

$P^*$ について整理していくと

$\displaystyle \begin{aligned} P^* &= A P^+ A^T + B Q B^T \\ &= A \{ (I - G^* C^T) P^* \} A^T + B Q B^T \\ &= A P^* A^T - A \{ P^* C (C^T P^* C + R)^{^-1} \} C^T P^* A^T + B Q B^T \\ &= A P^* A^T - A P^* C (C^T P^* C + R)^{^-1} C^T P^* A^T + B Q B^T \\ \end{aligned}$

まとめると、

$\displaystyle A P^* A^T - P^* - A P^* C (C^T P^* C + R)^{^-1} C^T P^* A^T + B Q B^T = 0$

この方程式の解となる [tex: P^ ] を求めれば、
そこから、カルマンゲイン [tex: G^ ] と事後共分散行列 $P^+$ の収束値も求めることができる

離散時間リカッチ代数方程式

上記の方程式を解く上で、
離散時間リカッチ代数方程式（discrete-time algebraic Riccati equation）
を解く関数が、python の controlモジュールに dare として提供されている

dare では、入力 $A, B, Q, R$ から下記の $X$ を求めることができる

$\displaystyle A^T X A - X - A^T X B (B^T X B + R)^{^-1} B^T X A + Q = 0$

比較すると、

$\displaystyle \begin{matrix} A P^* A^T &-& P^* &-& A P^* C (C^T P^* C + R)^{^-1} C^T P^* A^T &+& B Q B^T &=& 0 \\ A^T X A &-& X &-& A^T X B (B^T X B + R)^{^-1} B^T X A &+& Q &=& 0 \\ \end{matrix}$

dare の関数に以下の入力をすることで、事後共分散行列の収束値 $P^*$ を求めることができる

$\displaystyle \begin{aligned} A &\leftarrow A^T \\ B &\leftarrow C \\ Q &\leftarrow B Q B^T \\ R &\leftarrow R \\ \end{aligned}$

計算例

前回の制御入力ありの状態モデルで、実際に、収束値を求めてみる
入力に対する行列 $B_u$ は、収束値に影響を与えないが、
入力ノイズに対する行列 $B$ は影響する

$\displaystyle \begin{aligned} A &= \begin{bmatrix} 0 & -0.7 \\ 1 & -1.5 \\ \end{bmatrix} \\ B_u &= B = \begin{bmatrix} 0.5 \\ 1 \\ \end{bmatrix} \\ C^T &= \begin{bmatrix} 0 & 1 \end{bmatrix} \\ Q & = 0.01 \\ R & = 0.1 \\ \end{aligned}$

import matplotlib.pyplot as plt
import numpy as np
import pandas as pd

A_ = np.array([[0, -0.7], [1, -1.5]])
Bu = np.array([[0.5], [1.]])
B_ = np.array([[0.5], [1.]])
C_ = np.array([[0.], [1.]])
Q_ = np.array([[0.01]])
R_ = np.array([[0.1]])

import control.matlab as pyctrl

Pm_conv, _, _ = pyctrl.dare(A_.T, C_, B_ @ Q_ @B_.T, R_)
print('Pm_conv = \n', Pm_conv)

G_conv = Pm_conv @ C_ @ (C_.T @ Pm_conv @ C_ + R_)**-1
print('G_conv = \n', G_conv)

P_conv = (np.eye(A_.shape[0]) - G_conv @ C_.T) @ Pm_conv
print('P_conv = \n', P_conv)

Pm_conv = 
 [[0.01243089 0.01686667]
 [0.01686667 0.02541876]]
G_conv = 
 [[0.13448283]
 [0.20267113]]
P_conv = 
 [[0.01016261 0.01344828]
 [0.01344828 0.02026711]]

事後共分散行列の値が、前回の数値シミュレーションの最終値とほぼ一致している

計算値による数値シミュレーション

計算で求めたカルマンゲインの収束値を用いてシミュレーションで推定を行う

def kf_conv(A_, Bu, C_, u_, y_, xhat, G_conv):
    xhatm = A_ @ xhat + Bu @ u_
    xhat_new = xhatm + G_conv @ (y_ - C_.T @ xhatm)
    return xhat_new

N_ = 100
np.random.seed(0)

v_ = np.array([[np.random.randn() * np.sqrt(Q_[0][0])]])  # システム雑音
w_ = np.array([[np.random.randn() * np.sqrt(R_[0][0])]])  # 観測雑音
u_arr = []

x_arr = [np.array([[0.], [0.]])]
y_arr = [C_.T @ x_arr[0] + w_]

xhat_arr = [np.array([[0.], [0.]])]

for i in range(1, N_):
    v_ = np.array([[np.random.randn() * np.sqrt(Q_[0][0])]])  # システム雑音
    w_ = np.array([[np.random.randn() * np.sqrt(R_[0][0])]])  # 観測雑音
    u_ = np.array([[np.sign(np.random.randn())]])             # 制御入力
    u_arr += [u_]

    x_ = A_ @ x_arr[-1] + Bu @ u_ + B_ @ v_
    y_ = C_.T @ x_ + w_
    x_arr += [x_]
    y_arr += [y_]

    xhat = kf_conv(
        A_, Bu, C_, u_, y_, xhat_arr[-1], G_conv)
    xhat_arr += [xhat]

u_arr    = np.array(u_arr)
x_arr    = np.array(x_arr)
y_arr    = np.array(y_arr)
xhat_arr = np.array(xhat_arr)

fig = plt.figure(figsize=(6, 5))

ax1 = fig.add_subplot(3, 1, 1)
ax1.step(np.arange(1, N_), u_arr[:, 0, 0], where='pre', ls='-', label='$u$')
ax1.set_ylabel('$u$')
ax1.grid(ls=':'); ax1.legend()

ax2 = fig.add_subplot(3, 1, 2)
ax2.plot(xhat_arr[:, 0, 0], ls='-', label='$\hat{x_1}$')
ax2.plot(x_arr[:, 0, 0], ls='--', label='$x_1$')
ax2.set_ylabel('$x_1$')
ax2.grid(ls=':'); ax2.legend()

ax3 = fig.add_subplot(3, 1, 3)
ax3.plot(xhat_arr[:, 1, 0], ls='-', label='$\hat{x_2}$')
ax3.plot(x_arr[:, 1, 0], ls='--', label='$x_2$')
ax3.plot(y_arr[:, 0, 0], ls='-', alpha=0.6, label='$y$')
ax3.set_xlabel('$k$')
ax3.set_ylabel('$x_2$')
ax3.grid(ls=':'); ax3.legend()

fig.tight_layout()

前回と同様に、推測できていることが分かる
最初から、カルマンゲインが最適化されているため、
1サンプル目から良好な結果が得られている

おまけ各パラメータの次元

状態モデルとカルマンフィルタのパラメータの次元を整理しておく

状態モデル

状態変数 $x(k)$ が $n$ 次元ベクトル（ $(n,1)$ 行列）
入力 $u(k)$ （入力ノイズ $\nu$ ）が $m$ 次元ベクトル（ $(m,1)$ 行列）
出力（観測量） $y(k)$ が $l$ 次元ベクトル（ $(l,1)$ 行列）

$\displaystyle A は (n,n) \\ B は (n,m) \\ C は (n,l) \\ \begin{aligned} \begin{bmatrix} x_1(k+1) \\ \vdots \\ x_n(k+1) \\ \end{bmatrix} &= \begin{bmatrix} A_{1,1} & \cdots & A_{1,n} \\ \vdots & \ddots & \vdots \\ A_{n,1} & \cdots & A_{n,n} \\ \end{bmatrix} \begin{bmatrix} x_1(k) \\ \vdots \\ x_n(k) \\ \end{bmatrix} + \begin{bmatrix} B_{1,1} & \cdots & B_{1,m} \\ \vdots & \ddots & \vdots \\ B_{n,1} & \cdots & B_{n,m} \\ \end{bmatrix} \begin{bmatrix} \nu_1 \\ \vdots \\ \nu_m \\ \end{bmatrix} \\ \begin{bmatrix} y_1(k) \\ \vdots \\ y_l(k) \\ \end{bmatrix} &= \begin{bmatrix} C_{1,1} & \cdots & C_{n,1} \\ \vdots & \ddots & \vdots \\ C_{1,l} & \cdots & C_{n,l} \\ \end{bmatrix} \begin{bmatrix} x_1(k) \\ \vdots \\ x_n(k) \\ \end{bmatrix} + \begin{bmatrix} \omega_1 \\ \vdots \\ \omega_l \\ \end{bmatrix} \\ \end{aligned}$

カルマンフィルタ

$\displaystyle \begin{matrix} P^-(k) &=& A & P(k-1) & A^T &+& B & Q & B^T \\ [n \times n] &=& [n \times n] & [n \times n] & [n \times n] &+& [n \times m] & [m \times m] & [m \times n] \\ \end{matrix}$

$\displaystyle \begin{matrix} G(k) &=& P^-(k) & C &(& C^T & P^-(k) & C &+& R &)^-1 \\ [n \times l] &=& [n \times n] & [n \times l] &(& [l \times n] & [n \times n] & [n \times l] &+& [l \times l] &)^-1 \\ \end{matrix}$

$\displaystyle \begin{matrix} \hat{x}(k) &=& \hat{x}^-(k) &+& G(k) &(& y(k) &-& C^T & \hat{x}^-(k) &) \\ [n \times 1] &=& [n \times 1] &+& [n \times l] &(& [l \times 1] &-& [l \times n] & [n \times 1] &) \\ \end{matrix}$

$\displaystyle \begin{matrix} P(k) &=& (& I &-& G(k) & C^T &)& P^-(k) \\ [n \times n] &=& (& [n \times n] &-& [n \times l] & [l \times n] &)& [n \times n] \\ \end{matrix}$

具体例

4状態変数、3入力、2出力
$n=4, \quad m=3, \quad l=2$

$\displaystyle \begin{aligned} \begin{bmatrix} x_1(k+1) \\ x_2(k+1) \\ x_3(k+1) \\ x_4(k+1) \\ \end{bmatrix} &= \begin{bmatrix} A_{1,1} & A_{1,2} & A_{1,3} & A_{1,4} \\ A_{2,1} & A_{2,2} & A_{2,3} & A_{2,4} \\ A_{3,1} & A_{3,2} & A_{3,3} & A_{3,4} \\ A_{4,1} & A_{4,2} & A_{4,3} & A_{4,4} \\ \end{bmatrix} \begin{bmatrix} x_1(k+1) \\ x_2(k+1) \\ x_3(k+1) \\ x_4(k+1) \\ \end{bmatrix} + \begin{bmatrix} B_{1,1} & B_{1,2} & B_{1,3} \\ B_{2,1} & B_{2,2} & B_{2,3} \\ B_{3,1} & B_{3,2} & B_{3,3} \\ B_{4,1} & B_{4,2} & B_{4,3} \\ \end{bmatrix} \begin{bmatrix} \nu_1 \\ \nu_2 \\ \nu_3 \\ \end{bmatrix} \\ \begin{bmatrix} y_1(k) \\ y_2(k) \\ \end{bmatrix} &= \begin{bmatrix} C_{1,1} & C_{2,1} & C_{3,1} & C_{4,1} \\ C_{1,2} & C_{2,2} & C_{3,2} & C_{4,2} \\ \end{bmatrix} \begin{bmatrix} x_1(k+1) \\ x_2(k+1) \\ x_3(k+1) \\ x_4(k+1) \\ \end{bmatrix} + \begin{bmatrix} \omega_1 \\ \omega_2 \\ \end{bmatrix} \\ \end{aligned}$

$\displaystyle \begin{aligned} P(k) &= \begin{bmatrix} P_{1,1} & P_{1,2} & P_{1,3} & P_{1,4} \\ P_{2,1} & P_{2,2} & P_{2,3} & P_{2,4} \\ P_{3,1} & P_{3,2} & P_{3,3} & P_{3,4} \\ P_{4,1} & P_{4,2} & P_{4,3} & P_{4,4} \\ \end{bmatrix} \\ G(k) &= \begin{bmatrix} G_{1,1} & G_{1,2} \\ G_{2,1} & G_{2,2} \\ G_{3,1} & G_{3,2} \\ G_{4,1} & G_{4,2} \\ \end{bmatrix} \\ Q &= \begin{bmatrix} Q_{1,1} & Q_{1,2} & Q_{1,3} \\ Q_{2,1} & Q_{2,2} & Q_{2,3} \\ Q_{3,1} & Q_{3,2} & Q_{3,3} \\ \end{bmatrix} \\ R &= \begin{bmatrix} R_{1,1} & R_{1,2} \\ R_{2,1} & R_{2,2} \\ \end{bmatrix} \\ \end{aligned}$