サイトマップ

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

制御工学 現代制御 カルマンフィルタ Python

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

今回は、前回の行列演算で使用する「逆行列補題」について確認する
また Python の Sympyモジュール を使用して、検算(?)も行う

逆行列の補助定理について

下記のサイトが詳しい
https://manabitimes.jp/math/1041

本来の定理自体は、下記になる

\displaystyle (A + BDC)^{-1} = A^{-1} - A^{-1} B (D^{-1} + C A^{-1} B)^{-1} C A^{-1}

カルマンフィルタでは、特に D = I の場合を考え、下記になる

\displaystyle (A + BC)^{-1} = A^{-1} - A^{-1} B (I + C A^{-1} B)^{-1} C A^{-1}

この定理の利点は、 A^{-1} を取り出せることだと思われる

導出

(A + BC) を掛けて、 I になることを確認すれば良い

\displaystyle \begin{align} & (A + BC)(A^{-1} - A^{-1} B (I + C A^{-1} B)^{-1} C A^{-1}) \\ &= I + BCA^{-1} - A A^{-1} B (I + C A^{-1} B)^{-1} C A^{-1} - B C A^{-1} B (I + C A^{-1} B)^{-1} C A^{-1} \\ &= I + BCA^{-1} - B (I) (I + C A^{-1} B)^{-1} C A^{-1} - B (C A^{-1} B) (I + C A^{-1} B)^{-1} C A^{-1} \\ &= I + BCA^{-1} - B (I + C A^{-1} B) (I + C A^{-1} B)^{-1} C A^{-1} \\ &= I + BCA^{-1} - B C A^{-1} \\ &= I \\ \end{align}

Sympyでの検算

2x2行列で各成分を見ていく

import numpy as np
import sympy as sp

シンボルの準備

a_s = np.array(sp.symbols('a_{1:3\,1:3}', real=True)).reshape(2, 2)
b_s = np.array(sp.symbols('b_{1:3\,1:3}', real=True)).reshape(2, 2)
c_s = np.array(sp.symbols('c_{1:3\,1:3}', real=True)).reshape(2, 2)

A = sp.Matrix(a_s)
B = sp.Matrix(b_s)
C = sp.Matrix(c_s)
display(A)
display(B)
display(C)
\displaystyle \begin{bmatrix} a_{1,1} & a_{1,2} \\ a_{2,1} & a_{2,2} \end{bmatrix}
\displaystyle \begin{bmatrix} b_{1,1} & b_{1,2} \\ b_{2,1} & b_{2,2} \end{bmatrix}
\displaystyle \begin{bmatrix} c_{1,1} & c_{1,2} \\ c_{2,1} & c_{2,2} \end{bmatrix}

両辺を計算

res1 = (A + B * C).inv()
res2 = A.inv() - A.inv() * B * (sp.eye(2) + C * A.inv() * B).inv() * C * A.inv()

下記で、成分を確認できる（結果は長くなるので省略）

display(res1[0, 0])
display(res2[0, 0])

ただし、この状態だと、式が複雑すぎて、成り立っているか判断できない
sympy の simplify を使うと数式を整理できる

res3 = sp.simplify(res1)
res4 = sp.simplify(res2)

同じく、 下記で確認できる

display(res3[0, 0])
display(res4[0, 0])

今度は、同じになっていることが分かる

下記で 0 になることも確認できる

res3 - res4
\displaystyle \begin{bmatrix} 0 & 0 \\ 0 & 0 \end{bmatrix}

具体的な数値でも確認

A は固定して、B, C を乱数で生成する

A = sp.Matrix([
    [1, 2],
    [3, 4]
])
B = sp.Matrix(np.random.randint(-10, 10, (2, 2)))
C = sp.Matrix(np.random.randint(-10, 10, (2, 2)))

display(A)
display(B)
display(C)
\displaystyle \begin{bmatrix} 1 & 2 \\ 3 & 4 \end{bmatrix}
\displaystyle \begin{bmatrix} 4 & -10 \\ -9 & 7 \end{bmatrix}
\displaystyle \begin{bmatrix} 2 & 6 \\ 5 & 5 \end{bmatrix} 
res1 = (A + B * C).inv()
display(res1)
\displaystyle \begin{bmatrix} - \frac{1}{73} & \frac{8}{365} \\ - \frac{4}{219} & - \frac{41}{1095} \end{bmatrix} 
res2 = A.inv() - A.inv() * B * (sp.eye(2) + C * A.inv() * B).inv() * C * A.inv()
display(res2)
\displaystyle \begin{bmatrix} - \frac{1}{73} & \frac{8}{365} \\ - \frac{4}{219} & - \frac{41}{1095} \end{bmatrix}

こちらでも、（当然だが）両辺が同じになり、成り立っていることが確認できる

参考文献

この記事は以下の書籍を参考にしましたが、
私の拙い知識で書いておりますので、誤り等ありましたらご指摘ください

カルマンフィルタの基礎

カルマンフィルタの基礎

Amazon