/**
 *    ballistic_coefficient_estimation.cpp
 *
 * 拡張カルマンフィルタ(Extended Kalman Filter)をを応用して弾道係数を推定する
 *
 **/

#include <stdio.h>
#include <stdlib.h>
#include <math.h>

#define G (9.799)                 /* 重力の加速度 [m/s^2] */

#define InitAlt (30000)           /* target 初期高度 [m] */
#define InitEstAlt (30010)        /* target 初期推定高度 [m]*/
#define InitVel (-3000)           /* target 初期速度 [m/s] */
#define InitEstVel (-3100)        /* target 初期推定速度 [m/s] */
#define BETA (2000)               /* target 弾道係数 [Kg/m^2] */
#define InitEstBETA (2500)        /* target 初期推定弾道係数 [Kg/m^2] */
#define InitEstAltCov (10*10)     /* target 初期高度推定誤差(共分散) [m^2] */
#define InitEstVelCov (100*100)   /* target 初期速度制定誤差(共分散) [m^2/s^2] */
#define InitEstBetCoc (1000*1000) /* target 初期弾道係数制定誤差(共分散) [kg/m^2] */
#define ProcessNoise (0)          /* 信号処理雑音のスペクトル密度 [(Kg/m^2)^2] */
#define DATA_OUT (2.5)            /* データ出力間隔：ts*DATA_OUT [s] */
#define YtDef (6418)              /* (dvtyd/dyt) の微係数 */
 
/* 正規ホワイト・ノイズ生成関数 */
void gauss(double signoise, double *xnoise);

int main()
{
	int order;                        /*マトリクスの次数*/
	int step;                         /*計算のステップ*/
	int i,j,k;                        /*for 文の繰り返し制御変数*/	

	double phi[4][4];                 /*システム基本マトリクス(Φ)*/
	double p[4][4],m[4][4];           /*推定値更新前及び更新後誤差マトリクス(P),(M)*/
	double phip[4][4],phipphit[4][4]; /*(ΦP)及び(ΦPΦ';Φ'転置)マトリクス*/
	double gain[4][2];                /*カルマン・ゲイン・マトリクス*/
	double q[4][4];                   /*雑音マトリクス(Q)*/
	double hmat[2][4],ht[4][2];       /*測定マトリクス(H)*/
	double hm[2][4],mht[4][2];        /*(HM)及び(MH')マトリクス*/
	double phit[4][4];                /*(Φ')マトリクス*/
	double hmht[2][2];                /*(HMH')マトリクス*/
	double kh[4][4];                  /*(KH)マトリクス*/
	double idn[4][4];                 /*単位マトリクス(identity Matrix;(I))*/
	double ikh[4][4];                 /*(I - KH)マトリクス*/
	double hmhtr,hmhtrinv;            /*(HMH'+ noise)及びその逆数*/
	double xnoise;                    /*関数gauss()のポインタ引数(正規雑音)[m]*/
	double ytdbar;                    /*'bar' (dyt/dt)の 値[m/s]*/
	double vtydbar;                   /*'bar' (dyty/dt)の 値[m/s^2]*/
	double res;                       /*target 高度測定残差[m]*/
	double yt,ythat;                  /*targetの高度及び同推定値[m]*/
	double vty,vtyhat;                /*targetの速度及び同推定値[m/s]*/
    double erry,errv;                 /*(yt-ythat)[m]及び(vty-vtyhat)[m/s]*/
	double beta,betahat;              /*弾道係数(β)及び同推定値[Kg/m^2]*/
	double errbeta;                   /*(beta - betahat)*/
	double sp11,sp22,sp33;            /*p[1][1],p[2][2],p[3][3] の平方根*/
	double ytk;                       /*yt の [Km] 表示*/
	double ts;                        /*サンプリング時間[s]*/
	double tf;                        /*経過時間(time of flight)[s]*/
	double q33;                       /*雑音のスペクトル密度*/
	double t;                         /*target 測定時間[s]*/
	double s;                         /*サンプリング周期監視変数*/
	double h;                         /*積分時間刻み幅[s]*/
	double signoise;                  /*測定誤差の分散の平方根[m]*/
	double ytold,vtyold;              /*１計算ステップ前の yt, vty の値*/
	double ytd,vtyd;                  /*(dyt/dt:速度)[m/s] 及び (dvty/dt:加速度[m/^2])*/
	double rhoh;                      /*大気密度(ρ)[Kg/m^3]*/
	double f21,f22,f23;               /*System Dynamics Matrix (F)2行の元 */
	double ytdef;					  /*(dvtyd/dyt) の微係数*/
	double dout;                      /*データ出力間隔[s]*/

	FILE *fptr1;
	FILE *fptr2;
	
	fptr1 = fopen("DATFIL.TXT","w");
	fptr2 = fopen("COVFIL.TXT","w");

	yt = InitAlt;
	vty = InitVel;

	ythat = InitEstAlt;
	vtyhat = InitEstVel;

	beta = BETA;
	betahat = InitEstBETA;

	order = 3;

	ts = 0.05;
	tf = 30.0;

	q33 = (ProcessNoise * ProcessNoise) / tf;

	t = 0.0;
	s = 0.0;
	h = 0.001;
	dout = 0.0;
	
	signoise=sqrt(InitEstAltCov);

	for (i=1; i<=order; i++){
		for (j=1; j<=order; j++){
			phi[i][j] = 0.0;
			p[i][j] = 0.0;
			q[i][j] = 0.0;
			idn[i][j] = 0.0;
		}
	}

 	idn[1][1] = 1.0;
	idn[2][2] = 1.0;
	idn[3][3] = 1.0;

	p[1][1] = signoise * signoise;
	p[2][2] = InitEstVelCov;
	p[3][3] = InitEstBetCoc;

	for (i=1; i<=order; i++){
		hmat[1][i] = 0.0;
		ht[i][1] = 0.0;
	}

	hmat[1][1] =1.0;
	ht[1][1] = 1.0;
	
	ytdef = YtDef;

	while(yt >= 0.0)
	{
		ytold = yt;
		vtyold = vty;

 		step = 1;

		do
		{
			ytd = vty;

            rhoh = 2.202 * exp(-0.1558 * yt / 1000) / G;
            
			vtyd = rhoh * G * vty * vty / (2.0 * beta) - G;

			switch(step)
			{
				case 1:
					step = 2;

 					yt = yt + h * ytd;
					vty = vty + h * vtyd;

 					t = t + h;
					break;

				case 2:
					step = 1;

 					yt = 0.5 * (ytold + yt + h * ytd);
					vty =0.5 * (vtyold + vty + h * vtyd);
				    s = s + h;
				    dout = dout + h;

				  	if(s > (ts - 0.00001)) 
				  	{
						s = 0.0;

						rhoh = 2.2020 * exp(-0.1558 * ythat / 1000) / G;

						f21 = -G * rhoh * vtyhat * vtyhat / (2.0 * ytdef * betahat);
						f22 = rhoh * G * vtyhat / betahat;
						f23 = -rhoh * G * vtyhat * vtyhat / (2.0* betahat * betahat);

						phi[1][1] = 1.0;
						phi[1][2] = ts;

						phi[2][1] = f21 * ts;
						phi[2][2] = 1.0 + f22 * ts;
						phi[2][3] = f23 * ts;
						phi[3][3] = 1.0;

						q[2][2] = f23 * f23 * q33 * ts * ts * ts / 3.0;
						q[2][3] = f23 * q33 * ts * ts / 2.0;

						q[3][2] = q[2][3];
						q[3][3] = q33 * ts;

						for (i=1; i<=order; i++){
							for (j=1; j<=order; j++){
								phit[j][i] = phi[i][j];
							}
						}

						for (i=1; i<=order; i++){
							for (j=1; j<=order; j++){
								phip[i][j] = 0.0;
								for (k=1; k<=order; k++){
									phip[i][j] = phip[i][j] + phi[i][k] * p[k][j];
								}
							}
						}
 
						for (i=1; i<=order; i++){
							for (j=1; j<=order; j++){
								phipphit[i][j] = 0.0;
								for (k=1; k<=order; k++){
									phipphit[i][j] = phipphit[i][j] + phip[i][k] * phit[k][j];
								}
							}
						}

						for (i=1; i<=order; i++){
							for (j=1; j<=order; j++){
								m[i][j] = phipphit[i][j] + q[i][j];
							}
						}
 
						for (i=1; i<=1; i++){
							for (j=1; j<=order; j++){
								hm[i][j] = 0.0;
								for (k=1; k<=order; k++){
									hm[i][j] = hm[i][j] + hmat[i][k] * m[k][j];
								}
							}
						}

						for (i=1; i<=1; i++){
							for (j=1; j<=1; j++){
								hmht[i][j] = 0.0;
								for (k=1; k<=order; k++){
									hmht[i][j] = hmht[i][j] + hm[i][k] * ht[k][j];
								}
							}
						}

						hmhtr = hmht[1][1] + signoise * signoise;
						hmhtrinv = 1.0 / hmhtr;

						for (i=1; i<=order; i++){
							for (j=1; j<=1; j++){
								mht[i][j] = 0.0;
								for (k=1; k<=order; k++){
									mht[i][j] = mht[i][j] + m[i][k] * ht[k][j];
								}
							}
						}

						for (i=1; i<=order; i++){
							gain[i][1] = mht[i][1] * hmhtrinv;
						}

						for (i=1; i<=order; i++){
							for (j=1; j<=order; j++){
								kh[i][j] = 0.0;
								for (k=1; k<=1; k++){
									kh[i][j] = kh[i][j] + gain[i][k] * hmat[k][j];
								}
							}
						}

						for (i=1; i<=order; i++){
							for (j=1; j<=order; j++){
								ikh[i][j] = idn[i][j] - kh[i][j];
							}
						}

						for (i=1; i<=order; i++){
							for (j=1; j<=order; j++){
								p[i][j] = 0.0;
								for (k=1; k<=order; k++){
									p[i][j] = p[i][j] + ikh[i][k] * m[k][j];
								}
							}
						}

					/* 関数呼び出し */
 						gauss(signoise,&xnoise);

						ytdbar = vtyhat;

						rhoh = 2.2020 * exp(-0.1558 * ythat / 1000) / G;
						vtydbar = rhoh * G * vtyhat * vtyhat / (2.0 * betahat) - G;

						res = yt + xnoise - (ythat + ytdbar * ts);
						ythat = ythat + ytdbar * ts + gain[1][1] * res;
						vtyhat = vtyhat + vtydbar * ts + gain[2][1] * res;
						betahat = betahat + gain[3][1] * res;

						if(dout > (ts * DATA_OUT - 0.00001))
						{
							dout = 0;

							erry = yt - ythat;
							errv = vty - vtyhat;
							errbeta = beta - betahat;

							sp11 = sqrt(p[1][1]);
							sp22 = sqrt(p[2][2]);
							sp33 = sqrt(p[3][3]);

							ytk = yt / 1000.0;

							printf("%10.4f %10.4f %10.4f %10.4f %10.4f %10.4f %10.4f %10.4f\n"
			                   ,t,ytk,yt,ythat,vty,vtyhat,beta,betahat);
							fprintf(fptr1,"%10.4f %10.4f %10.4f %10.4f %10.4f %10.4f %10.4f %10.4f\n"
			                   ,t,ytk,yt,ythat,vty,vtyhat,beta,betahat);
							fprintf(fptr2,"%10.4f %10.4f %10.4f %10.4f %10.4f %10.4f %10.4f %10.4f %10.4f %10.4f %10.4f\n"
			                   ,t,ytk,erry,sp11,-sp11,errv,sp22,-sp22,errbeta,sp33,-sp33);
						} /* if */

					} /* if */
					break;

				default:;
			} /* switch - case */
		}while( step >= 2); /* do - while */
	} /* whilw */

 	fclose (fptr1);
 	fclose (fptr2);
	return 0;
}

/* 正規白色雑音(Gaussian White Noise) 生成関数 */

void gauss(double signoise,double *xnoise)
{
	double sum,x,y,temp;
	int j;

	sum=0.;

	for (j=1; j<=6; j++){
		x = rand();
		y = x / RAND_MAX;
		sum = sum + y;
	}
	temp = sum - 3.0;
	*xnoise = 1.414 * temp * signoise;
}