ルンゲクッタで試してみる about hakoniwa-px4sim

[tmori]

前提

以下の２つの関数をそのまま利用する。

• drone_physics::acceleration_in_body_frame()
• drone_physics::angular_acceleration_in_body_frame()

[tmori]

コード設計

上記どちらの関数も３次元のベクトルなので、３次元ベクトル変換関数(f_vec)と思うことにする。

変数定義

• c_vec：現在の状態
• k1_vec：k2を求めるための状態
• k2_vec：k3を求めるための状態
• k3_vec：k4を求めるための状態
• delta_time：シミュレーション時間精度
• n_vec：次の時間の状態

[tmori]

k1

k1 = f_vec(c_vec);

k2

k2_vec = {c_vec.x + 0.5 * k1.x * delta_time, c_vec.y + 0.5 * k1.y * delta_time, c_vec.z + 0.5 * k1.z * delta_time};
k2 = f_vec(k2_vec);

k3

k3_vec = {c_vec.x + 0.5 * k2.x * delta_time, c_vec.y + 0.5 * k2.y * delta_time, c_vec.z + 0.5 * k2.z * delta_time};
k3 = f_vec(k3_vec);

k4

k4_vec = {c_vec.x + k3.x * delta_time, c_vec.y + k3.y * delta_time, c_vec.z + k3.z * delta_time};
k4 = f_vec(k4_vec);

[tmori]

次の状態

n_vec = {
c_vec.x + (delta_time / 6.0) * (k1.x + 2 * k2.x + 2 * k3.x + k4.x),
c_vec.y + (delta_time / 6.0) * (k1.y + 2 * k2.y + 2 * k3.y + k4.y),
c_vec.z + (delta_time / 6.0) * (k1.z + 2 * k2.z + 2 * k3.z + k4.z)
};

[tmori]

というわけで修正してみます。

[tmori]

めちゃくちゃ冗長だけど、やってみた。

1e8b264