拡張カルマンフィルタ
実装については多くのwebサイトで取り上げられているため、そこまで難しくはないでしょう。
本記事では、まず理論的な証明から始め
Pythonで3次元倒立振子の姿勢を拡張カルマンフィルタを用いて推定し、状態フィードバックで倒立制御のシミュレーションをします。
理論：線形カルマンフィルタ
より
\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$
としたとき、
予測による時間更新
\begin{align}
\hat{x}(k|Y(k-1)) &=& A \hat{x}(k-1|Y(k-1)) + Bu(k-1)
\end{align}
測定による更新則
\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$の更新
P(k|Y(k-1)) = AP(k-1|Y(k-1)) A^T +DWD^T\tag{4}
P(k|Y(k))^{-1} = P(k|Y(k-1))^{-1} +C^T V^{-1} C\tag{3}
とわかっている。
の逆行列補題を用いて変形すると。
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)
さらに、
を参考にしているため、少し文字を変更する。
文字変更
カルマンゲイン$K_k$を次のように定義する。
K_k = P(k|k-1)C^TS_k^{-1}\\
S_k = CP(k|k-1)C^T+V\\
すると、
\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}
に変形できます。
こちらでも同様の結果になっています。
私の以前書いた、線形カルマンフィルタの導出の結果では
K_k = P(k|k)C^TV^{-1}
としましたが、
K_k = P(k|k-1)C^T(V+CP(k|k-1)C^T)^{-1}
が正しい方法なのだと思います。
結構悩んだのですが、二つの式は同値ではないという結論になりました。
実際にシミュレーションする際に、二つの場合それぞれでやってみます。
まとめ
1.初期値
\hat{x(0)}\\
P(0|0) = E[e(0)e(0)^T]
の二つを与える。
2.同定済みのシステムモデルに基づいて、次の時間ステップの予測を行う。
\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.計測値に基づいて、予測値の補正を行う。
\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}
以上の繰り返しを行う。
非線形カルマンフィルタの場合
基本的に、推定状態量を起点にして、システムを線形化し、線形カルマンフィルタの適用をするという流れです。
流れとしては
- $\hat{x(k|k)}$を用いてシステムの線形化、$A,Bu,C,D$の導出
- 1step後の予測を行う。
- 計測によって補正する
以上の繰り返しになります。
線形化について
https://www.jstage.jst.go.jp/article/sicejl/56/9/56_638/_pdf
を参考にしました
\begin{eqnarray}
\dot{x} = f_{continuous}(x,u)+v\\
y = h_{continuous}(x) + w\\
\end{eqnarray}
が支配方程式の時、まず、サンプリング時間で離散化します。
\begin{eqnarray}
x_{k+1} = f(x_k,u_k)+v_k\\
y_k = h(x_k)+w_k\\
\end{eqnarray}
推定値$\hat{x_k}$中心で、線形化
\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}
さらに、定数ベクトルを隔離
\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$と名付ける
\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となづける
A= \frac{\partial f}{\partial x}\\
C= \frac{\partial h }{\partial x}
まとめると
\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.初期値
\begin{eqnarray}
\hat{x(0)}\\
P(0|0) = E[e(0)e(0)^T]
\end{eqnarray}
の二つを与える。
2.$\hat{x(0)}$で線形化する。
\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.同定済みのシステムモデルに基づいて、次の時間ステップの予測を行う。
\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.計測値に基づいて、予測値の補正を行う。
\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}
の流れになります。
#三次元振子に対する、シミュレーション
###運動方程式
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$を使用します。
###運動学由来
\dot{q} = \frac{1}{2} q \times w_v
で計算できます。
###線形化
非線形システムの離散化がわからないので、先に線形化してから、離散化（dt掛けるだけ）しようと思います。
I\dot{w} =-Cw + N_A - \tilde{r_{AP}} C_{OA}^T(q) (-mg)e_z
を変形し
\dot{w} =-I^{-1}Cw + I^{-1}N_A - I^{-1}\tilde{r_{AP}} C_{OA}^T(q) (-mg)e_z
非線形部を微分します。
\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}
よって、
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}
)
線形化ができました。
\dot{q} = \frac{1}{2} q \times w_v
の方も線形化します。
\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}
よって、
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}\\
と離散化されます。二つをまとめると
\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$をサンプリング間隔とします。
x_{k+1} = (A \Delta t+I) x_k + bk \Delta_t
と離散化します。
シミュレーション結果
Pythonでシミュレーションします。
コードは最後に乗せてます。
gif画像なので、画像をクリックして別ウィンドウで表示すれば、動画で見れます。
青：実現象での姿勢
赤：EKFを用いて推定した姿勢
初期値を実際の減少から、10度ずれた状態にして推定をしましたが、最終的にほとんど一致して推定できています。
z方向の回転については、EKFでの補正がされず、積分誤差・初期値ずれが積み重なるので、z方向のずれがずっと残ります。
実際、60度擦れた状態でシミュレーションをすると
z方向の回転については実際のシステムと一致できていません。
V,Wの決定方法
ちょっと変えてみましたが、対して変化はなかったです。
シミュレーションコード
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')