線形回帰分析を用いたロケットの頂点時間の導出①[C言語,Arduino]

追記：
次の記事(成功しました！)
線形回帰分析を用いたロケットの頂点時間の導出②[C言語,Arduino]

1. 概要

この記事は，時間と気圧高度のデータから水ロケットの軌道を推定し，頂点時間を計算する手法を解説したものです．
結論から言うと，今回作成したプログラムでは水ロケットのような短時間で頂点高度に到達するものでは上手く行きませんでした．
　しかしハイブリッドロケットなど長時間飛行するようなロケットなどではうまくいくことが期待されるので，とりあえずメモとして残します．

もうしばらくやる予定なのでまた今度もう少し良いモデルを考えて実験します．
GitHubのリポジトリ作っときましたので参考までに．コードはここに投げていきます．

• TonyMooori/WaterRocket: Control water rocket by Arduino.

3.式の導出

まずロケットの挙動がどうなっているか，ロケットのシミュレーションを行います．今回はOpenRocketを用いて行います．ただ，エンジンにペットボトルは選択できないので適当に作ったロケットで見てみます．

ロケットの挙動は大まかに見ると「エンジン点火」→「加速」→「エンジン燃焼終了」→「自由落下運動」→「パラシュート開傘」→「一定速度で落下」という挙動を示します．実際には空力が働いているので自由落下運動では無いのですが，抗力の小さいロケットなら概ね自由落下運動として見て良いと思います．
　自由落下運動ならば，高度 $y$ をプロットしてみると放物線運動となり， $$y = a t^2 + b t + c$$ という式となることが予想されます．実際に，上のグラフを見てもエンジン燃焼終了後は概ね放物線運動をしていることが読み取れるかと思います．
　放物線運動(二次関数)のパラメータは3つなので，3点のデータがあれば連立方程式を解いて $a,b,c$ は求まり，頂点に到達する時刻 $t=-\frac{b}{2a}$ を計算することができます．
　しかし，センサーから読み取られたデータはノイズがある程度存在するため，正しい結果が得られない可能性があります．そこで今回は線形回帰分析の知識を使って， $a,b,c$ を推定してみます．
　センサーから得られた高度と時間のデータ ${(t_1,y_1),...,(t_n,y_n)}$ があります．ここでこれらのデータが放物線 $$y = a t^2 + b t + c$$ にノイズ $e_i$ が乗ったものとして考えていきます．
　ノイズ $e_i$ は元の関数との差なので $$e_i=a t_i^2 + b t_i +c - y_i$$ と表す事ができます．ここでノイズの二乗和を評価関数 $J$ として定義します．(普通の最小二乗法をします) $$J = \frac{1}{2} \sum_{i=1}^{n} (a t_i^2 + b t_i +c - y_i)^2$$ 　
　この評価関数を最小とするようなパラメータ $a,b,c$ が，データに一番フィットするような放物線のパラメータと成るはずです．評価関数 $J$ を$a,b,c$ について偏微分したものが0となる連立方程式を考えます．

$$\begin{eqnarray*} \frac{\partial J}{\partial a} &=& \sum_{i=1}^n(a t_i^2 + bt_i + c - y_i )t_i^2 = 0 \\ \frac{\partial J}{\partial b} &=& \sum_{i=1}^n(a t_i^2 + bt_i + c - y_i )t_i = 0 \\ \frac{\partial J}{\partial c} &=& \sum_{i=1}^n(a t_i^2 + bt_i + c - y_i )= 0 \\ \end{eqnarray*}$$
　行列形式にすれば， $$( \sum_{i=1}^n \left[ \begin{array}{rrr} t_i^4 & t_i^3 & t_i^2 \\ t_i^3 & t_i^2 & t_i \\ t_i^2 & t_i & 1 \end{array} \right] ) \left[ \begin{array}{rrr} a \\ b \\ c \end{array} \right] = ( \sum_{i=1}^n\left[ \begin{array}{rrr} t_i^2 y_i \\ t_i y_i \\ y_i \end{array} \right] )$$ 　となりこの連立方程式をとけば評価関数 $J$ について最適なパラメータ $a,b,c$ がわかります．   (※データ数が少なかったり運が悪いと正則行列でない場合があるのでそうならないようにお祈りしましょう(?))
　また，地球の地表付近で観測された放物線運動ならば $a$ は 重力加速度 $g$ を用いればおよそ ${-\frac{1}{2}g}$ 付近であるはずです． 計算した結果のパラメータが正しいかどうかを判定するのに使えるかと思います

3.シミュレーション

どの程度データ数があれば十分かや，実装したプログラムが正しいかどうかを確認する意味も含めてシミュレーションを行いました．
シミュレーションでは高度のデータにノイズとして±0.5mの乱数を加えています．また，あとで示すArduinoのプログラムではループ時間が約0.03秒であったので，それに従ってシミュレーションを行いました．
プログラムはこんな感じ．REPマクロとか修正してなかったけど大丈夫でしょう．余分にライブラリincludeしてるけど気にしないでください．

	#include <iostream>
	#include <vector>
	#include <algorithm>
	#include <map>
	#include <string>
	#include <cstdio>
	#include <cstdlib>
	#include "TopDetector.h"
	#define INF 99999999
	#define FOR(i,a,b) for (int i=(a);i<(b);i++)
	#define RFOR(i,a,b) for (int i=(b)-1;i>=(a);i--)
	#define REP(i,n) for (int i=0;i<(n);i++)
	#define RREP(i,n) for (int i=(n)-1;i>=0;i--)
	#define MAX_ERROR 0.5 // 誤差の最大値[m]
	using namespace std;
	// [min_x,max_x)の一様乱数を返す
	float random(float min_x,float max_x);
	// f(x) = a ( t - top_time ) ^ 2 + max_height
	float parabola(float t,float a,float top_time,float max_height);
	// シミュレーションを行う
	void test(float a,float top_time,float max_height,float from_time,float wait_time,int N);
	int main(void){
	float a = -4.9;
	int n_datas[5] = {10,30,50,100,500};
	srand(1);
	printf("%8s","a");
	printf("%8s","top_t");
	printf("%8s","from_t");
	printf("%8s","wait_t");
	printf("%8s","N");
	printf("%8s","a ans");
	printf("%10s\n","top_t ans");
	REP(i,60)
	cout << "-";
	cout << endl;
	// t^2の係数, 最高到達時間，最高高度，開始時間，サンプリング周波数，サンプリング数
	REP(i,5)
	test(a, 25.0, 30, 20.0, 0.03, n_datas[i]);
	REP(i,5)
	test(a, 25.0, 30, 15.0, 0.03, n_datas[i]);
	REP(i,5)
	test(a, 55.0, 100, 40.0, 0.03, n_datas[i]);
	return 0;
	}
	// [min_x,max_x)の一様乱数を返す
	float random(float min_x,float max_x){
	return (max_x-min_x)*float(rand())/(RAND_MAX + 1)+min_x;
	}
	// f(x) = a ( t - top_time ) ^ 2 + max_height
	float parabola(float t,float a,float top_time,float max_height){
	float delta_t = t - top_time;
	return a * (delta_t * delta_t) + max_height;
	}
	// シミュレーションを行う
	void test(float a,float top_time,float max_height,float from_time,float wait_time,int N){
	TopDetector td;
	float t,y;
	REP(i,N){
	t = from_time + wait_time * i;
	y = parabola(t,a,top_time,max_height) + random(-MAX_ERROR,MAX_ERROR);
	td.push( t , y );
	}
	td.calc();
	printf("%8.2lf",a);
	printf("%8.2lf",top_time);
	printf("%8.2lf",from_time);
	printf("%8.2lf",wait_time);
	printf("%8d",N);
	printf("%8.2lf",td.get_a());
	printf("%8.2lf\n",td.get_top_time());
	}

view raw test.cpp hosted with ❤ by GitHub

	#ifndef TOP_DETECTOR_HEADER_FILE
	#define TOP_DETECTOR_HEADER_FILE
	class TopDetector {
	public:
	// コンストラクタ
	TopDetector() {
	reset();
	}
	// 変数の初期化を行うメソッド
	void reset() {
	for (int i = 0 ; i < 3 ; i++ ) {
	_x[i] = 0.0;
	_b[i] = 0.0;
	for (int j = 0 ; j < 3 ; j++ ) {
	_A[i][j] = 0.0;
	}
	}
	_n_data = 0;
	}
	// データを追加するメソッド
	void push(float t, float y) {
	const float t0 = 1;
	const float t1 = t0 * t;
	const float t2 = t1 * t;
	const float t3 = t2 * t;
	const float t4 = t3 * t;
	_A[0][0] += t4;
	_A[0][1] += t3;
	_A[0][2] += t2;
	_A[1][0] += t3;
	_A[1][1] += t2;
	_A[1][2] += t1;
	_A[2][0] += t2;
	_A[2][1] += t1;
	_A[2][2] += t0;
	_b[0] += t2 * y;
	_b[1] += t1 * y;
	_b[2] += t0 * y;
	_n_data++;
	}
	// データが有れば放物線の係数を計算するメソッド
	void calc() {
	if ( _n_data == 0 )
	return;
	solve<3>(_A, _x, _b);
	}
	// 追加されたデータ数を返すメソッド
	int get_n_data() {
	return _n_data;
	}
	// 計算済みならばt^2のパラメータを返すメソッド
	float get_a() {
	return _x[0];
	}
	// 計算済みならばt^1のパラメータを返すメソッド
	float get_b() {
	return _x[1];
	}
	// 計算済みならばt^0のパラメータを返すメソッド
	float get_c() {
	return _x[2];
	}
	// 頂点時間
	float get_top_time() {
	if ( _x[1] == 0.0 )
	return 1e+10;
	else
	return -_x[1] / (2.0f * _x[0]);
	}
	private:
	template<int N>
	void solve(const float M[N][N], float x[N], const float b[N]) {
	float A[N][N];
	float B[N];
	float temp;
	// コピー
	for(int i = 0 ; i < N ; i++ ){
	B[i] = b[i];
	for(int j = 0 ; j < N ; j++ ){
	A[i][j] = M[i][j];
	}
	}
	for (int i = 0 ; i < N ; i++ ) {
	// ピボット選択
	for (int j = i + 1 ; j < N ; j++ ) {
	if ( abs(A[i][j]) >= 1e-8 )
	break;
	for (int k = 0 ; k < N ; k++ )
	A[i][k] += A[j][k];
	B[i] += B[j];
	}
	// 対角成分を1に
	temp = A[i][i];
	for (int j = i ; j < N ; j++ )
	A[i][j] /= temp;
	B[i] /= temp;
	// 前進消去
	for (int j = i + 1 ; j < N ; j++ ) {
	temp = A[j][i];
	for (int k = i ; k < N ; k++ )
	A[j][k] -= temp * A[i][k];
	B[j] -= temp * B[i];
	}
	}
	// 後進消去
	for (int i = N - 1 ; i >= 0 ; i-- ){
	for (int j = i - 1 ; j >= 0 ; j-- ){
	B[j] -= A[j][i] * B[i];
	}
	}
	for(int i = 0 ; i < N ; i++ )
	x[i] = B[i];
	}
	float _A[3][3]; // 係数行列
	float _b[3]; // 出力ベクトル
	float _x[3]; // 解ベクトル
	int _n_data;
	};
	#endif

view raw TopDetector.h hosted with ❤ by GitHub

結果はこんな感じです．Markdownはいいぞ……．

上のグラフを見てない人も見た人も，なんかヤヴァい値が見えますね．

データの個数が100個以下ではうまくいくときもあれば，上手く行かないときもあるというような非常に怖い結果が出ています．500個のデータが有ればどれも上手く言っているようですが，少なくとも水ロケットでは使い物にならなそうです．

なぜなのかはデータをプロットしてやれば明確で，実際に三番目のテストケースのデータの個数が100の場合のやつをプロットしてみるとこうなります．

時間が短すぎてもはや直線で，放物線には見えないですね，これでは値が意味不明になるのもわかります．こればっかりはどうしようもないので，次はもう少し制約の強いモデルを考えて見ますので次回の記事を読んでもらうことを期待します……．

4.Arduinoのコード

一応ペットボトルロケットでやってみましたが上手く行かなかったです．だいぶ長くなるのでGitHubのリンクを貼っときます．

• WaterRocket/experiment3 at master · TonyMooori/WaterRocket

センサーはmpu9250とbmp180です．加速度のノルムが一定数以上になった場合，サンプリング周期0.03sで高度データを取得，30個データがたまった時点で計算を行います．

もう一度いいますがこれは上手く行かなかったやつなので，また別のプログラムを作る予定ですが，数学モデルが変わるのでこの負の遺産を残しておきます．

5.結論とか

上手く行かなかったのは多分制約が弱すぎるからなので，とりあえずa=-4.9の制約を入れてどうなるかやって，それでだめなら $$\frac{dy}{dt} = -gt + b$$  を基底関数としてやったりしてみます．

追記：
次の記事(成功しました！)
線形回帰分析を用いたロケットの頂点時間の導出②[C言語,Arduino]

2017年最初の投稿

ちょっと遅いですが，新年明けましておめでとうございます．

ブログを初めて15ヶ月ぐらいですかね，去年の続きです．
( ※2016年最初の投稿　http://tony-mooori.blogspot.jp/2016/01/2016.html )

1.昨年を振り返って

時系列に思い出していきますと，2016年1月ぐらいに本格的にツイッターをはじめましたね……
ニコニコ動画でオセロの動画をアップロードして，反応した人を数人フォローしましたのがきっかけですかね……
あとそのころOpenCVとかで遊んでいた気がします．

で，2月ぐらいにこれをやった．

2週間前のネタを今更掘り返す……@donnay1224さんや@motcho_twに刺激されてフーリエ級数でミクを描いてみた．せっかくなのでツイッターに上げる pic.twitter.com/2Pg4OGwdJf

— もりとにー (@TonyMooori) 2016年2月16日

巡回セールスマン問題をごまかしながら適当に解いて，x方向y方向にフーリエ級数展開してアレヤコレヤやったやつですね，この頃は楽しかったですね……

この頃はまだなんかこう，高校生だった頃の活気というか何かがあって，僕が最強なんだみたいなところがあったので無限に恥ずかしいですね……本当は無能なのに……

その後3月の春休みでは『これなら分かる最適化数学』を読みました．これ案外人生を変えた本かもしれないですね，まだちゃんと扱える(それでも誤魔化し誤魔化しですが)のは線形回帰分析ぐらいですが，非常に強いツールでたいへん助かっています．

とくにこの　最小二乗法による球面フィッティングを用いた磁気センサのオフセットの計算　は書いててすごく良かったです，めっちゃ楽しい．

あとパノラマ動画の傾き補正を行うプログラムをサークルで作ったのですが誰も使ってくれなかったですね，近いうちにVectorさんにあげておきたいです．
（余談ですが水玉コラ作成ソフトをVectorに上げましたが，Vectorさんの対応がとても良かったです）

8月にサークルとして参加している某大会は惨敗でしたが，あるていど構造物の設計ができるようになったので2017年度は活躍したいですね．

設計ができるようになったので，パラレルリンクロボットを作ったのはいい経験でした．トルクが足りず使い物にならなかった奴は，今机として有効活用されています(*˘︶˘*).｡.:*♡．

パラレルリンクロボットのテスト動画．円状の軌跡で動かす． pic.twitter.com/MzcZSWXja4

— もりとにー (@TonyMooori) 2016年9月18日

後は競技プログラミングをすこし勉強して僕には才能がないんだと悟ったぐらいですかね．

あとHaskellを触りましたがお金の出し惜しみをしたせいで本が手元になくてあまり勉強できていないですね……悲しい．

2.去年できるようになったこと

一応確認してある程度満足したいので書いてみます．

• 線形回帰分析(ちょっと)
• ちょっとした物の設計
• Twitter（これが多分最強です)
• GitHub(git commit git push しかわからない)
• C(『猫でもわかるC言語入門』を再勉強したら理解が深まった)
• C++(vectorなどの標準ライブラリの一部慣れた)
• Q学習(大学の授業のライントレーサーで使ってみてPIDでいいやってなった程度)
• 古典制御(いま勉強中，まだちゃんと設計や評価できない)
• シミュレーション(RK実装できるようになったけどscipyのodeintでいいやってなってる)

こんぐらいですかね，どれをとっても中途半端．まぁでも多少成長を感じてます．

3. 去年の目標達成率

2016年最初の投稿(　http://tony-mooori.blogspot.jp/2016/01/2016.html )で，上げた3つのやつです．

・ニューラルネットワークの実装（DNNを含む）
→三層の順伝播は式覚えてるけど逆伝播わっかんね．DNN?なにそれえっちいの売ってるとこ？

・数学の能力を上げる
→『これなら分かる最適化数学』読んだので○

・大会・コンテストなどに沢山出る
→モノづくり系は一個も出てない，サークルの大会だけだった．AtCoderとかやってみたけど全然ダメ．でもプロ生勉強会には出たから外には出れたかな(?)

ダメダメですね．今年は頑張ります．

4. 今年の目標

全部達成できたら俺結婚するんだ……(フラグ)

・数学の能力を上げる……電情はやってる離散数学とか触って置きたい
・関数型言語の勉強……Haskellよくわかんないけど大事な気がする
・大会やニコ動にアウトプットしてく……大会とかコンテストとかに出ような(?)
・本を読む……教養が必要だと感じるふしがあった気がするので

とりあえず目標を4つにしました．

あまり面白いことが言えない管理人ですが，今年も何卒ご贔屓に．