拡張カルマンフィルタのシミュレーション

# 拡張カルマンフィルタ
実装については多くのwebサイトで取り上げられているため、そこまで難しくはないでしょう。
本記事では、まず理論的な証明から始め
Pythonで3次元倒立振子の姿勢を拡張カルマンフィルタを用いて推定し、状態フィードバックで倒立制御のシミュレーションをします。


# 理論：線形カルマンフィルタ

https://qiita.com/hiRina/items/42b37748606acac256ef

より

```math
\begin{align*}
\boldsymbol{x}_k &= \boldsymbol{Ax}_k + \boldsymbol{Bu}_k + D w_k\\
\boldsymbol{y}_k &= \boldsymbol{Cx}_k+v_k
\end{align*}
```
のシステムに対し、

$v,w$は

* 白色雑音
* 全ての信号と独立
* 平均$0$
* 共分散$E[v v^T] = V, E[w w^T] = W$

としたとき、

予測による時間更新

```math
\begin{align}
\hat{x}(k|Y(k-1)) &=&  A \hat{x}(k-1|Y(k-1)) + Bu(k-1)
\end{align}
```

測定による更新則

```math
\hat{x}(k|Y(k))  = \hat{x}(k|Y(k-1)) + P(k|Y(k))C^TV^{-1} (y(k) - C \hat{x}(k|Y(k-1)))
```

推定誤差共分散$P$の更新

```math
P(k|Y(k-1)) = AP(k-1|Y(k-1)) A^T +DWD^T\tag{4}
```


```math
P(k|Y(k))^{-1} =  P(k|Y(k-1))^{-1}  +C^T V^{-1} C\tag{3}
```

とわかっている。

https://manabitimes.jp/math/1041

の逆行列補題を用いて変形すると。

```math
P(k|k) =  P(k|k-1) - P(k|k-1) C^T (V+CP(k|k-1) C^T)^{-1} C P(k|k-1)
```

さらに、

https://tajimarobotics.com/extended-kalman-filter-localization/

を参考にしているため、少し文字を変更する。

### 文字変更
カルマンゲイン$K_k$を次のように定義する。

```math
K_k = P(k|k-1)C^TS_k^{-1}\\
S_k = CP(k|k-1)C^T+V\\
```

すると、

```math
\begin{eqnarray}
P(k|k) =  P(k|k-1) -　K_k C P(k|k-1)\\
=(I - K_k C)P(k|k-1)\\
\end{eqnarray}
```
に変形できます。

https://qiita.com/Crafty_as_a_Fox/items/55448e2ed9ce0f340814

こちらでも同様の結果になっています。
私の以前書いた、線形カルマンフィルタの導出の結果では

```math
K_k = P(k|k)C^TV^{-1}

```
としましたが、

```math
K_k = P(k|k-1)C^T(V+CP(k|k-1)C^T)^{-1}
```

が正しい方法なのだと思います。

結構悩んだのですが、二つの式は同値ではないという結論になりました。
実際にシミュレーションする際に、二つの場合それぞれでやってみます。

### まとめ

1.初期値

```math
\hat{x(0)}\\
P(0|0) = E[e(0)e(0)^T]
```
の二つを与える。

2.同定済みのシステムモデルに基づいて、次の時間ステップの予測を行う。

```math
\begin{align}
\hat{x}(k|Y(k-1)) &=& A \hat{x}(k-1|Y(k-1)) + Bu(k-1)\\
P(k|Y(k-1)) &=& AP(k-1|Y(k-1)) A^T +DWD^T\tag{4}
\end{align}
```
$k=1$から始めて、$\hat{x(1|0)},P(1|0)$が計算される。

3.計測値に基づいて、予測値の補正を行う。

```math
\begin{eqnarray}
K_k = P(k|k-1)C^TS_k^{-1}\\
S_k = CP(k|k-1)C^T+V\\
P(k|k) = (I - K_k C)P(k|k-1)\\
\hat{x}(k|Y(k))  = \hat{x}(k|Y(k-1)) + K_k (y(k) - C \hat{x}(k|Y(k-1)))
\end{eqnarray}
```

以上の繰り返しを行う。

# 非線形カルマンフィルタの場合
基本的に、推定状態量を起点にして、システムを線形化し、線形カルマンフィルタの適用をするという流れです。

流れとしては

1. $\hat{x(k|k)}$を用いてシステムの線形化、$A,Bu,C,D$の導出
2. 1step後の予測を行う。
3. 計測によって補正する

以上の繰り返しになります。

### 線形化について
https://www.jstage.jst.go.jp/article/sicejl/56/9/56_638/_pdf
を参考にしました

```math
\begin{eqnarray}
\dot{x} = f_{continuous}(x,u)+v\\
y = h_{continuous}(x) + w\\
\end{eqnarray}
```
が支配方程式の時、まず、サンプリング時間で離散化します。

```math
\begin{eqnarray}
x_{k+1} = f(x_k,u_k)+v_k\\
y_k = h(x_k)+w_k\\
\end{eqnarray}
```
推定値$\hat{x_k}$中心で、線形化

```math
\begin{eqnarray}
x_{k+1} = f(\hat{x_k},u_k)+ \frac{\partial f}{\partial x} (x_k - \hat{x_k}) + v_k\\
y_k = h(\hat{x_k})+\frac{\partial h }{\partial x} (x_k - \hat{x_k}) +w_k\\
\end{eqnarray}
```

さらに、定数ベクトルを隔離

```math
\begin{eqnarray}
x_{k+1} = \frac{\partial f}{\partial x} x_k + 定数ベクトル( f(\hat{x_k},u_k)- \frac{\partial f}{\partial x} \hat{x_k}) + w_k\\
y_k = \frac{\partial h }{\partial x} x_k +  定数ベクトル(h(\hat{x_k})-\frac{\partial h }{\partial x}  \hat{x_k} )+v_k\\
\end{eqnarray}

```

定数ベクトル部分を$d_k,b_k$と名付ける

```math
\begin{eqnarray}
b_k(\hat{x_k},u_k) = f(\hat{x_k},u_k)- \frac{\partial f}{\partial x} \hat{x_k}\\
d_k(\hat{x_k}) = h(\hat{x_k})-\frac{\partial h }{\partial x}  \hat{x_k}
\end{eqnarray}
```
微分した行列をA,Cとなづける

```math
A= \frac{\partial f}{\partial x}\\
C= \frac{\partial h }{\partial x}
```

まとめると

```math
\begin{eqnarray}
x_{k+1} = A x_k + b_k(\hat{x_k},u_k) + w_k\\
y_k = C x_k + d_k(\hat{x_k})+v_k\\
\end{eqnarray}
```

以上で線形化の完了です。

### EKFの流れ
$b_k,d_k$が増えたので、推定ステップの動作が少し変わります。


1.初期値

```math
\begin{eqnarray}
\hat{x(0)}\\
P(0|0) = E[e(0)e(0)^T]
\end{eqnarray}
```
の二つを与える。

2.$\hat{x(0)}$で線形化する。

```math
\begin{eqnarray}
x_{k+1} = A x_k + b_k(\hat{x_k},u_k) + w_k\\
y_k = C x_k + d_k(\hat{x_k})+v_k\\
\end{eqnarray}
```

3.同定済みのシステムモデルに基づいて、次の時間ステップの予測を行う。

```math
\begin{align}
\hat{x}(k|Y(k-1)) &=& A \hat{x}(k-1|Y(k-1)) + b_k\\
P(k|Y(k-1)) &=& AP(k-1|Y(k-1)) A^T +DWD^T\tag{4}
\end{align}
```
$k=1$から始めて、$\hat{x(1|0)},P(1|0)$が計算される。

4.計測値に基づいて、予測値の補正を行う。

```math
\begin{eqnarray}
K_k = P(k|k-1)C^TS_k^{-1}\\
S_k = CP(k|k-1)C^T+V\\
P(k|k) = (I - K_k C)P(k|k-1)\\
\hat{x}(k|Y(k))  = \hat{x}(k|Y(k-1)) + K_k (y(k) - C \hat{x}(k|Y(k-1)) - d_k)
\end{eqnarray}
```

の流れになります。

# 三次元振子に対する、シミュレーション

### 運動方程式

```math
I\dot{w} =-Cw + N_A - \tilde{r_{AP}} C_{OA}^T(q) (-mg)e_z
```
Iは慣性マトリクス
Cは粘性行列
N_Aは入力トルク
$\tilde{r_{AP}}$は支点$P$へのベクトルの外積行列
$C_{OA}^T(q)$はクォータニオンを用いて計算された、回転行列
となります。

状態変数としては、角速度$w$とクォータニオン$q$を使用します。

### 運動学由来

```math
\dot{q} = \frac{1}{2} q \times w_v
```

で計算できます。

### 線形化
非線形システムの離散化がわからないので、先に線形化してから、離散化（dt掛けるだけ）しようと思います。

```math
I\dot{w} =-Cw + N_A - \tilde{r_{AP}} C_{OA}^T(q) (-mg)e_z
```
を変形し

```math
\dot{w} =-I^{-1}Cw + I^{-1}N_A - I^{-1}\tilde{r_{AP}} C_{OA}^T(q) (-mg)e_z
```
非線形部を微分します。

```math
\begin{align}
- I^{-1}\tilde{r_{AP}} C_{OA}^T(q) (-mg)e_z &=&
mg I^{-1}\tilde{r_{AP}} 
\begin{pmatrix}
2(q_z q_x -q_y q_w)\\
2(q_y q_z +q_x q_w)\\
q_w^2 - q_x^2 - q_y^2 + q_z^2\\
\end{pmatrix}\\
&=&
mg I^{-1}\tilde{r_{AP}} (
\begin{pmatrix}
-2\hat{q_y}&2\hat{q_z}& -2\hat{q_w} & 2\hat{q_x}\\
2\hat{q_x}&2\hat{q_w}& 2\hat{q_z} & 2\hat{q_y}\\
2\hat{q_w}& -2\hat{q_x} & -2\hat{q_y} & 2\hat{q_z}\\
\end{pmatrix}
\begin{pmatrix}
q_w\\
q_x\\
q_y\\
q_z\\
\end{pmatrix}

+\begin{pmatrix}
2(\hat{q_z}\hat{ q_x} -\hat{q_y}\hat{ q_w})\\
2(\hat{q_y}\hat{ q_z} +\hat{q_x}\hat{ q_w})\\
\hat{q_w}^2 - \hat{q_x}^2 - \hat{q_y}^2 + \hat{q_z}^2\\
\end{pmatrix}

-
\begin{pmatrix}
-2\hat{q_y}&2\hat{q_z}& -2\hat{q_w} & 2\hat{q_x}\\
2\hat{q_x}&2\hat{q_w}& 2\hat{q_z} & 2\hat{q_y}\\
2\hat{q_w}& -2\hat{q_x} & -2\hat{q_y} & 2\hat{q_z}\\
\end{pmatrix}
\begin{pmatrix}
\hat{q_w}\\
\hat{q_x}\\
\hat{q_y}\\
\hat{q_z}\\
\end{pmatrix}
)
\end{align}
```
よって、

```math
A_1= mg I^{-1}\tilde{r_{AP}} 
\begin{pmatrix}
-2\hat{q_y}&2\hat{q_z}& -2\hat{q_w} & 2\hat{q_x}\\
2\hat{q_x}&2\hat{q_w}& 2\hat{q_z} & 2\hat{q_y}\\
2\hat{q_w}& -2\hat{q_x} & -2\hat{q_y} & 2\hat{q_z}\\
\end{pmatrix}\\
bk_1 = mg I^{-1}\tilde{r_{AP}} (
\begin{pmatrix}
2(\hat{q_z}\hat{ q_x} -\hat{q_y}\hat{ q_w})\\
2(\hat{q_y}\hat{ q_z} +\hat{q_x}\hat{ q_w})\\
\hat{q_w}^2 - \hat{q_x}^2 - \hat{q_y}^2 + \hat{q_z}^2\\
\end{pmatrix}

-
\begin{pmatrix}
-2\hat{q_y}&2\hat{q_z}& -2\hat{q_w} & 2\hat{q_x}\\
2\hat{q_x}&2\hat{q_w}& 2\hat{q_z} & 2\hat{q_y}\\
2\hat{q_w}& -2\hat{q_x} & -2\hat{q_y} & 2\hat{q_z}\\
\end{pmatrix}
\begin{pmatrix}
\hat{q_w}\\
\hat{q_x}\\
\hat{q_y}\\
\hat{q_z}\\
\end{pmatrix}
)

```
線形化ができました。


```math
\dot{q} = \frac{1}{2} q \times w_v
```
の方も線形化します。

```math
\begin{align}
\dot{q} &=& \frac{1}{2} 
\begin{pmatrix}
-q_x w_x -q_y w_y -q_z w_z\\
q_w w_x -q_z w_y + q_y w_z\\
q_z w_x +q_w w_y -q_x w_z\\
-q_y w_x + q_x w_y + q_w w_z\\
\end{pmatrix}\\

&=&
 \frac{1}{2} 
\begin{pmatrix}
-\hat{q_x} & -\hat{q_y}& -\hat{q_z}  &    0      & -\hat{w_x}& -\hat{w_y}& -\hat{w_z}\\
 \hat{q_w} & -\hat{q_z}&  \hat{q_y}  &  \hat{w_x}&      0    &  \hat{w_z}& -\hat{w_y}\\
 \hat{q_z} &  \hat{q_w}& -\hat{q_x}  &  \hat{w_y}& -\hat{w_z}&      0    & \hat{w_x}\\
-\hat{q_y} &  \hat{q_x}&  \hat{q_w}  &  \hat{w_z}&  \hat{w_y}& -\hat{w_x}&      0    \\
\end{pmatrix}
\begin{pmatrix}
w_x\\
w_y\\
w_z\\
q_w\\
q_x\\
q_y\\
q_z\\
\end{pmatrix}

+
\frac{1}{2} 
\begin{pmatrix}
-\hat{q_x} \hat{w_x} -\hat{q_y} \hat{w_y} -\hat{q_z} \hat{w_z}\\
\hat{q_w} \hat{w_x} - \hat{q_z} \hat{w_y} + \hat{q_y} \hat{w_z}\\
\hat{q_z} \hat{w_x} +\hat{q_w} \hat{w_y} -\hat{q_x} \hat{w_z}\\
-\hat{q_y} \hat{w_x} + \hat{q_x} \hat{w_y} + \hat{q_w} \hat{w_z}\\
\end{pmatrix}
-\frac{1}{2}
\begin{pmatrix}
-\hat{q_x} & -\hat{q_y}& -\hat{q_z}  &    0      & -\hat{w_x}& -\hat{w_y}& -\hat{w_z}\\
 \hat{q_w} & -\hat{q_z}&  \hat{q_y}  &  \hat{w_x}&      0    &  \hat{w_z}& -\hat{w_y}\\
 \hat{q_z} &  \hat{q_w}& -\hat{q_x}  &  \hat{w_y}& -\hat{w_z}&      0    & \hat{w_x}\\
-\hat{q_y} &  \hat{q_x}&  \hat{q_w}  &  \hat{w_z}&  \hat{w_y}& -\hat{w_x}&      0    \\
\end{pmatrix}
\begin{pmatrix}
\hat{w_x}\\
\hat{w_y}\\
\hat{w_z}\\
\hat{q_w}\\
\hat{q_x}\\
\hat{q_y}\\
\hat{q_z}\\
\end{pmatrix}


\end{align}
```
よって、

```math
A_2 = \frac{1}{2} 
\begin{pmatrix}
-\hat{q_x} & -\hat{q_y}& -\hat{q_z}  &    0      & -\hat{w_x}& -\hat{w_y}& -\hat{w_z}\\
 \hat{q_w} & -\hat{q_z}&  \hat{q_y}  &  \hat{w_x}&      0    &  \hat{w_z}& -\hat{w_y}\\
 \hat{q_z} &  \hat{q_w}& -\hat{q_x}  &  \hat{w_y}& -\hat{w_z}&      0    & \hat{w_x}\\
-\hat{q_y} &  \hat{q_x}&  \hat{q_w}  &  \hat{w_z}&  \hat{w_y}& -\hat{w_x}&      0    \\
\end{pmatrix}\\

bk_2 = \frac{1}{2} 
\begin{pmatrix}
-\hat{q_x} \hat{w_x} -\hat{q_y} \hat{w_y} -\hat{q_z} \hat{w_z}\\
\hat{q_w} \hat{w_x} - \hat{q_z} \hat{w_y} + \hat{q_y} \hat{w_z}\\
\hat{q_z} \hat{w_x} +\hat{q_w} \hat{w_y} -\hat{q_x} \hat{w_z}\\
-\hat{q_y} \hat{w_x} + \hat{q_x} \hat{w_y} + \hat{q_w} \hat{w_z}\\
\end{pmatrix}
-\frac{1}{2}
\begin{pmatrix}
-\hat{q_x} & -\hat{q_y}& -\hat{q_z}  &    0      & -\hat{w_x}& -\hat{w_y}& -\hat{w_z}\\
 \hat{q_w} & -\hat{q_z}&  \hat{q_y}  &  \hat{w_x}&      0    &  \hat{w_z}& -\hat{w_y}\\
 \hat{q_z} &  \hat{q_w}& -\hat{q_x}  &  \hat{w_y}& -\hat{w_z}&      0    & \hat{w_x}\\
-\hat{q_y} &  \hat{q_x}&  \hat{q_w}  &  \hat{w_z}&  \hat{w_y}& -\hat{w_x}&      0    \\
\end{pmatrix}
\begin{pmatrix}
\hat{w_x}\\
\hat{w_y}\\
\hat{w_z}\\
\hat{q_w}\\
\hat{q_x}\\
\hat{q_y}\\
\hat{q_z}\\
\end{pmatrix}\\
```
と離散化されます。二つをまとめると

```math
\frac{d}{dt}
\begin{pmatrix}
w_x\\
w_y\\
w_z\\
q_w\\
q_x\\
q_y\\
q_z\\
\end{pmatrix}
 = 
\begin{pmatrix}
-C& A_1\\
A_2\\
\end{pmatrix}
\begin{pmatrix}
w_x\\
w_y\\
w_z\\
q_w\\
q_x\\
q_y\\
q_z\\
\end{pmatrix}
+
\begin{pmatrix}
bk_1\\
bk_2\\
\end{pmatrix}
```
と線形化されます。



### 離散化
オイラー法で無理やり線形化します。$\Delta_t$をサンプリング間隔とします。

```math
x_{k+1} = (A \Delta t+I) x_k + bk \Delta_t
```
と離散化します。






# シミュレーション結果
Pythonでシミュレーションします。
コードは最後に乗せてます。
![10degf.gif](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/405484/d236789e-d40a-f4b7-c3c7-7669d145815e.gif)
gif画像なので、画像をクリックして別ウィンドウで表示すれば、動画で見れます。
青：実現象での姿勢
赤：EKFを用いて推定した姿勢

初期値を実際の減少から、10度ずれた状態にして推定をしましたが、最終的にほとんど一致して推定できています。

z方向の回転については、EKFでの補正がされず、積分誤差・初期値ずれが積み重なるので、z方向のずれがずっと残ります。

実際、60度擦れた状態でシミュレーションをすると
![60deg.gif](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/405484/7e6d2303-d23b-f525-b461-a5e81b4ac996.gif)

z方向の回転については実際のシステムと一致できていません。

### V,Wの決定方法
ちょっと変えてみましたが、対して変化はなかったです。




### シミュレーションコード

```Python
import numpy as np
import matplotlib.pyplot as plt
import matplotlib.animation as animation


def quatanion(w,l,m,n):
    lam = np.array([l,m,n])
    norm = np.linalg.norm(lam)
    lam = lam/norm
    
    quatanion = np.append(np.cos(w/2),lam*np.sin(w/2))
    return quatanion[:,np.newaxis]

def cross(q,p)    :
    
    D = np.array([[q[0][0],-q[1][0],-q[2][0],-q[3][0]],
                  [q[1][0],q[0][0],-q[3][0],q[2][0]],
                  [q[2][0],q[3][0],q[0][0],-q[1][0]],
                  [q[3][0],-q[2][0],q[1][0],q[0][0] ]   ])
    
    
    #print(D.shape)
    #print(D)
    return np.dot(D,p)

def bar(q):
    hoge = np.array([q[0][0],-q[1][0],-q[2][0],-q[3][0]])

    return hoge[:,np.newaxis]    

def COA_matrix(q):
    q0 = q[0][0]
    q1 = q[1][0]
    q2 = q[2][0]
    q3 = q[3][0]
    hoge = np.array([[q0**2+q1**2-q2**2-q3**2,2*(q1*q2-q3*q0),2*(q3*q1+q2*q0)],
                     [2*(q1*q2+q3*q0),q2**2-q3**2-q1**2+q0**2,2*(q2*q3-q1*q0)],
                     [2*(q3*q1-q2*q0),2*(q2*q3+q1*q0),q3**2-q1**2-q2**2+q0**2]])
    return hoge

def Childe(r):
    r1 = r[0][0]
    r2 = r[1][0]
    r3 = r[2][0]
    
    hoge = np.array([[0,-r3,r2],
                     [r3,0,-r1],
                     [-r2,r1,0]])
    
    return hoge

def normalize(v, axis=-1, order=2):
    l2 = np.linalg.norm(v, ord = order, axis=axis, keepdims=True)
    l2[l2==0] = 1
    return v/l2

"""
時間
"""
step = 0
T = 4000
dt = 0.01

step_system = 10  #実システムの方は細かく動かす
dt_system  = dt/step_system


"""
初期値
"""
w_pre  = np.asarray([0,0,0])
w_pre = w_pre[:,np.newaxis]   

w_true = np.asarray([0,0,0])
w_true = w_true[:,np.newaxis]   

q_pre  = quatanion(90/180*np.pi,1,1,0)
q_true = quatanion(0,1,0,0)

P_0 = np.diag([0.1,0.1,0.1  ,0.1,0.1,0.1,0.1])

gif_name = 'V10_W.gif'

"""
推定値:この値は使わない。ただの宣言：必要ないが
"""
w_pre_next = np.asarray([0,0,0])
w_pre_next = w_pre_next[:,np.newaxis]
q_pre_next = quatanion(0,1,0,0)

"""
パラメータ
"""
I= np.array([[1,0,0],
             [0,1,0],
             [0,0,1]])
inv_I = np.linalg.inv(I)

C= np.array([[1.1,0,0],
             [0,1.1,0],
             [0,0,1.1]])
IC = np.dot(inv_I,C)

r_AP =  np.asarray([-1,-1,-1])
r_AP = r_AP[:,np.newaxis]   

Ir_AP = np.dot(inv_I,Childe(r_AP))

m = 0.5
g = 9.81
F_OA = np.asarray([0,0,-m*g])
F_OA = F_OA[:,np.newaxis]

N_A = np.asarray([0,0,0])
N_A = N_A[:,np.newaxis]

mgIr_AP = m*g*Ir_AP

C = np.eye(3)
hoge = np.zeros((3,4))
C = np.concatenate([C,hoge],1)
    
"""
カルマンフィルタ　EKF用パラメータ
"""
W = np.diag([0.1,0.1,0.1  ,0.1,0.1,0.1,0.1])
V = np.diag([0.1,0.1,0.1])*10

"""
グラフ用変数
"""
q_history = q_true
q_pre_history = q_pre

while True:
    print("step",step)
    """
    実システムの方の挙動
    """
    
    for i in range(step_system):
        d_w_true  = - np.dot(IC,w_true) - np.dot(inv_I,N_A) \
                            - np.dot(np.dot(Ir_AP,COA_matrix(q_true).T),F_OA)  #運動方程式
        w_true = w_true + d_w_true * dt_system
        
        wv = np.asarray([0,w_true[0][0],w_true[1][0],w_true[2][0]])
        wv = wv[:,np.newaxis]
        d_q_true = 0.5 *cross(q_true,wv)
        q_true = q_true + d_q_true * dt_system
        
        q_true = normalize(q_true,axis = 0)
        
    y = w_true    #計測によって、角速度が得られる。           
    
    """
    微分して、線形化する
    """
    qw = q_pre[0][0]
    qx = q_pre[1][0]
    qy = q_pre[2][0]
    qz = q_pre[3][0]
    
    wx = w_pre[0][0]
    wy = w_pre[1][0]
    wz = w_pre[2][0]
    
    
    
    A_w = np.array([[-2*qy,2*qz,-2*qw,2*qx],
                    [2*qx,2*qw,2*qz,2*qy],
                    [2*qw,-2*qx,-2*qy,2*qz]])
    
    
    hoge = np.dot(mgIr_AP ,  A_w)
    AC_w = np.concatenate([-IC,hoge],1)
    
    A_q =0.5* np.array([[-qx,-qy,-qz,0,-wx,-wy,-wz],
                    [qw,-qz,qy,wx,0,wz,-wy],
                    [qz,qw,-qx,wy,-wz,0,wx],
                    [-qy,qx,qw,wz,wy,-wx,0]])
    
    
    A = np.concatenate([AC_w,A_q],0)
    
    
    
    hoge2 = np.array([[2*(qz*qx - qy*qw)],
                      [2*(qy*qz+qx*qw)],
                      [qw**2-qx**2-qy**2+qz**2]])
    hoge = np.dot(mgIr_AP, hoge2 - np.dot(A_w,q_pre))
    
    hoge3=0.5*np.array([[-qx*wx-qy*wy-qz*wz],
                        [qw*wx-qz*wy+qy*wz],
                        [qz*wx+qw*wy-qx*wz],
                        [-qy*wx+qx*wy+qx*wz]])
    
    hoge3 = hoge3 - np.dot(A_q,np.concatenate([w_pre,q_pre],0))
    
                           
    
    bk= np.concatenate([hoge,hoge3],0)
    
    """
    離散化
    
    dot x = Ax + bk + v
    y = Cx + w
    
    を
    
    x_k+1 = (Adt + eye)x_k + bkdt +v
    y = Cx + w
    
    にする
    """
    
    A_d = A*dt + np.eye(7)
    bkdt = bk*dt
        
    
    """
    EKFを用いた推定部分
    """
    x_pre = np.concatenate([w_pre,q_pre],0)
    x_next = np.dot(A_d,x_pre)+bkdt
    
    P_next = np.dot(np.dot(A_d,P_0),A_d.T)+W
    
    #予測値の補正
    S = np.dot(np.dot(C,P_next),C.T) + V
    K = np.dot(np.dot(P_next,C.T),np.linalg.inv(S))
    P_hosei = np.dot((np.eye(7) - np.dot(K,C)),P_next)
    x_hosei = x_next + np.dot(K,y - np.dot(C,x_next))
    
    """
    更新部
    """
    P_0 = P_hosei
    w_pre = x_hosei[0:3]
    q_pre = x_hosei[3:8]
    
    norm = np.linalg.norm(q_pre)
    q_pre = q_pre / norm
    
    
    """
    グラフ用に保存
    """
    q_history = np.append(q_history,q_true,axis = 1)
    q_pre_history = np.append(q_pre_history,q_pre,axis = 1)
    
    step = step + 1
    
    if step>=T:
        break
    

"""
グラフ化
"""
    


"""
3次元で gif化する
"""
fig = plt.figure(figsize=(10,10))
ax = fig.add_subplot(111, projection='3d')

# x, y, z成分のデータの作成

# x =  np.array([0,1,0,0,0,0,0,1])
# y =  np.array([0,0,0,1,0,0,1,0])
# z =  np.array([0,0,1,0,0,1,0,0])

# x =  np.array([0,1,0,0,0,0])
# y =  np.array([0,0,0,1,0,0])
# z =  np.array([0,0,1,0,0,1])

x =  np.array([0  ,0  ,1  ,0  ,0  ,0])
y =  np.array([0  ,0  ,0  ,0  ,1  ,0.5])
z =  np.array([0.5,0  ,0  ,1  ,0  ,0])

#ax.plot(x, y, z, color='blue')
#ax.scatter(x, y, z, color='blue')

min_range = -1
max_range = 1
ax.set_xlim(min_range, max_range)
ax.set_ylim(min_range, max_range)
ax.set_zlim(min_range, max_range)

ims = []

for i in range(300):

    """
    save_qに従い，座標変換行列CAOを作り，らせんの点を移動しながら，プロットする
    """
    COA = COA_matrix(q_history.T[i*10].reshape(4,1))
    rotate_x = []
    rotate_y = []
    rotate_z = []
    for j in range(6):
        vec_xyz = np.array([x[j],y[j],z[j]])
        vec_xyz =vec_xyz.reshape([3,1])
        vec_xyz = np.dot(COA,vec_xyz)
        rotate_x = np.append(rotate_x,vec_xyz[0][0])
        rotate_y = np.append(rotate_y,vec_xyz[1][0])
        rotate_z = np.append(rotate_z,vec_xyz[2][0])

    im1, = ax.plot(rotate_x, rotate_y, rotate_z, color='blue')
    
    COA = COA_matrix(q_pre_history.T[i*10].reshape(4,1))
    rotate_x2 = []
    rotate_y2 = []
    rotate_z2 = []
    for j in range(6):
        vec_xyz = np.array([x[j],y[j],z[j]])
        vec_xyz =vec_xyz.reshape([3,1])
        vec_xyz = np.dot(COA,vec_xyz)
        rotate_x2 = np.append(rotate_x2,vec_xyz[0][0])
        rotate_y2 = np.append(rotate_y2,vec_xyz[1][0])
        rotate_z2 = np.append(rotate_z2,vec_xyz[2][0])

    im2, = ax.plot(rotate_x2, rotate_y2, rotate_z2, color='red')
    
    ims.append([im1,im2])


ani = animation.ArtistAnimation(fig, ims, interval=100)
plt.show()

ani.save(gif_name, writer='pillow')

```