#include "ukfPDR.h"
#include <stdio.h>
#include <math.h>
#include <stdlib.h>
#include <string.h>

static float Psr[4][4];
static float PsrTmp[4][4];
static float ChiX[4][9];
static float ChiXtmp[4][9];
static float ChiY[3][9];
static float ChiYcov[3][3];
static float YXcov[4][3];
static float K[4][3];



void UKF_para(int L, float alpha, float beta, float kappa, float *gamma, float *wm, float *wc)
{
	//float lamda = alpha * alpha * (L + kappa) - L;
	//float lamda = -3.9996f;
	//*gamma = sqrt(L + lamda);
	*gamma = 0.0199999996f;


	for (int i = 0; i < 2 * L + 1; i++)
	{
		//wm[i] = 0.5 / (L + lamda);
		wm[i] = 1250.0f;
		//wc[i] = 0.5 / (L + lamda);
		wc[i] = 1250.0f;
	}

	//wm[0] = lamda / (L + lamda);
	wm[0] = -9999.0f;
	//wc[0] = lamda / (L + lamda) + 1 - alpha * alpha + beta;
	wc[0] = -9996.0f;
}

int matrixSubtract(float **leftMatrix, int leftxDimen, int leftyDimen, float **rightMatrix, int rightxDimen, int rightyDimen, float **dst)
{
	if (leftxDimen != rightxDimen || leftyDimen != rightyDimen)
	{
		printf("Dimension don't match");
		return -1;
	}

	int i = 0;
	int j = 0;

	for (i = 0; i < leftxDimen; i++)
		for (j = 0; j < leftyDimen; j++)
		{
			dst[i][j] = leftMatrix[i][j] - rightMatrix[i][j];

		}

	return 0;

}

void multiplyMatrix(float a[4][3], int axDimen, int ayDimen, float b[3][3], int bxDimen, int byDimen, float dst[4][3])
{

	int k = 0;

	float sum = 0.0f;

	for (int i = 0; i < axDimen; i++)
		for (int j = 0; j < byDimen; j++)
		{
			sum = 0.0f;

			for (k = 0; k < ayDimen; k++)
			{
				sum += a[i][k] * b[k][j];
			}

			dst[i][j] = sum;
		}

}

void invertMatrix(float src[3][3], float dst[3][3])
{
	float det;

	/* Compute adjoint: */
	dst[0][0] = +src[1][1] * src[2][2] - src[1][2] * src[2][1];
	dst[0][1] = -src[0][1] * src[2][2] + src[0][2] * src[2][1];
	dst[0][2] = +src[0][1] * src[1][2] - src[0][2] * src[1][1];
	dst[1][0] = -src[1][0] * src[2][2] + src[1][2] * src[2][0];
	dst[1][1] = +src[0][0] * src[2][2] - src[0][2] * src[2][0];
	dst[1][2] = -src[0][0] * src[1][2] + src[0][2] * src[1][0];
	dst[2][0] = +src[1][0] * src[2][1] - src[1][1] * src[2][0];
	dst[2][1] = -src[0][0] * src[2][1] + src[0][1] * src[2][0];
	dst[2][2] = +src[0][0] * src[1][1] - src[0][1] * src[1][0];

	/* Compute determinant: */
	det = src[0][0] * dst[0][0] + src[0][1] * dst[1][0] + src[0][2] * dst[2][0];

	/* Multiply adjoint with reciprocal of determinant: */
	det = 1.0f / det;

	dst[0][0] *= det;
	dst[0][1] *= det;
	dst[0][2] *= det;
	dst[1][0] *= det;
	dst[1][1] *= det;
	dst[1][2] *= det;
	dst[2][0] *= det;
	dst[2][1] *= det;
	dst[2][2] *= det;
}

void MeanSigmaMatrix(float **SigmaMatrix, float *meanSigma, float *weight, int stateDimen, int sigmaDimen)
{
	int i = 0;
	int j = 0;

	memset(meanSigma, 0, stateDimen * sizeof(float));

	for (i = 0; i < stateDimen; i++)
		for (j = 0; j < sigmaDimen; j++)
		{
			meanSigma[i] += SigmaMatrix[i][j] * weight[j];
		}

}

void StateMeasureCovariance(float covarianceMatrix[4][3], float state[4][9], int stateDimen, float measure[3][9], int mearsureDimen, float *wc, int length)
{
	int i = 0;
	int j = 0;
	int k = 0;

	float sum;

	for (i = 0; i < stateDimen; i++)
		for (j = 0; j < mearsureDimen; j++)
		{
			sum = 0.0;

			for (k = 0; k < length; k++)
			{
				sum += state[i][k] * wc[k] * measure[j][k];
			}

			covarianceMatrix[i][j] = sum;
		}
}

void updateSigmPoint(float **SigmaMatrix, float *meanSigma, int stateDimen, int SigmaDimen)
{
	int i = 0;
	int j = 0;

	for (i = 0; i < stateDimen; i++)
		for (j = 0; j < SigmaDimen; j++)
		{
			SigmaMatrix[i][j] -= meanSigma[i];
		}
}

void StateCovarianceMatrix(float stateCovarianceMatrix[4][4], float quatPredictMatrix[4][9], float *statePredict, float *wc, int length)
{
	int j = 0;
	int i = 0;
	int k = 0;
	float sum = 0.0f;

	for (j = 0; j < length; j++)
	{
		for (i = 0; i < 4; i++)
		{
			quatPredictMatrix[i][j] -= statePredict[i];
		}
	}

	for (i = 0; i < 4; i++)
		for (j = 0; j < 4; j++)
		{
			sum = 0.0f;

			for (k = 0; k < length; k++)
			{
				sum += quatPredictMatrix[i][k] * wc[k] * quatPredictMatrix[j][k];
			}
			stateCovarianceMatrix[i][j] = sum;
		}

	for (i = 0; i < 4; i++)
	{
		stateCovarianceMatrix[i][i] += 0.00001f;
	}


}

void QuatPredict(float quatPredictMatrix[4][9], float *quatPredict, float *wm, int length)
{
	int i = 0;
	
	memset(quatPredict, 0, 4 * sizeof(float));

	float sum = 0.0f;
	for (i = 0; i < 4; i++)
	{
		/*
		float sum = 0.0f;
		for (j = 0; j < length; j++)
		{
			sum += (quatPredictMatrix[i][j] * wm[j]);
		}
		 */
		quatPredict[i] = (quatPredictMatrix[i][0] * wm[0] + quatPredictMatrix[i][1] * wm[1] + quatPredictMatrix[i][2] * wm[2]
			+ quatPredictMatrix[i][3] * wm[3] + quatPredictMatrix[i][4] * wm[4] + quatPredictMatrix[i][5] * wm[5]
			+ quatPredictMatrix[i][6] * wm[6] + quatPredictMatrix[i][7] * wm[7] + quatPredictMatrix[i][8] * wm[8]);
		sum += quatPredict[i] * quatPredict[i];
	}

	sum = 1 / sqrt(sum);

	for (int i = 0; i < 4; i++)
	{
		quatPredict[i] *= sum;
	}

}

void MagPredict(float magPredictMatrix[3][9], float mag[3], float *wm, int length)
{
	int i = 0;
	int j = 0;
	memset(mag, 0, 3 * sizeof(float));

	for (i = 0; i < 3; i++)
	{
		for (j = 0; j < length; j++)
		{
			mag[i] += (magPredictMatrix[i][j] * wm[j]);
		}
	}
}


void UpdateQuat(float *gyr, float dt, float *q)
{
	float w1 = dt * gyr[0];
	float w2 = dt * gyr[1];
	float w3 = dt * gyr[2];

	float quatTmp[4];
	memcpy(quatTmp, q, 4 * sizeof(float));

	quatTmp[0] = q[0] - (q[1] * w1) * 0.5f - (q[2] * w2) * 0.5f - (q[3] * w3) * 0.5f;
	quatTmp[1] = q[1] + (q[0] * w1) * 0.5f + (q[2] * w3) * 0.5f - (q[3] * w2) * 0.5f;
	quatTmp[2] = q[2] + (q[0] * w2) * 0.5f - (q[1] * w3) * 0.5f + (q[3] * w1) * 0.5f;
	quatTmp[3] = q[3] + (q[0] * w3) * 0.5f + (q[1] * w2) * 0.5f - (q[2] * w1) * 0.5f;

	memcpy(q, quatTmp, 4 * sizeof(float));

}

void QuatNormalize(float *quat)
{
	int i = 0;
	float quatNorm = 1 / sqrt(quat[0] * quat[0] + quat[1] * quat[1] + quat[2] * quat[2] + quat[3] * quat[3]);

	if (isnan(quatNorm))
	{
		printf("四元数出现nan \n");
	}

	for (i = 0; i < 4; i++)
	{
		quat[i] *= quatNorm;
	}

}
void chol(float a_matrix[4][4], float b_matrix[4][4], float c_matrix[4][4], int ndimen)

//	输入参数:
//		b_matrix:  对称正定方阵    ndimen: 矩阵维数
//	返回值:
//		a_matrix: 下三角矩阵

{
	int i, j, r;
	float m = 0;

	for (i = 0; i < ndimen; i++)
	{
		for (j = 0; j < ndimen; j++)
			c_matrix[i][j] = 0;
	}

	c_matrix[0][0] = sqrt(b_matrix[0][0]);

	for (i = 1; i < ndimen; i++)
	{
		if (c_matrix[0][0] != 0)
			c_matrix[i][0] = b_matrix[i][0] / c_matrix[0][0];
		
	}

	for (i = 1; i < ndimen; i++)
	{
		for (r = 0; r < i; r++)		m = m + c_matrix[i][r] * c_matrix[i][r];

		c_matrix[i][i] = sqrt(b_matrix[i][i] - m);
		m = 0.0;

		for (j = i + 1; j < ndimen; j++)
		{
			for (r = 0; r < i; r++)		m = m + c_matrix[i][r] * c_matrix[j][r];
			
			c_matrix[j][i] = (b_matrix[i][j] - m) / c_matrix[i][i];
			m = 0;
		}
	}

	for (i = 0; i < ndimen; i++)
	{
		for (j = 0; j < ndimen; j++)
			a_matrix[i][j] = c_matrix[i][j];
	}
}

void quatXmag(float ChiX[4][9], float* mag, float ChiY[3][9])
{
	float dcm[3][3];
	float qin[4];

	for (int j = 0; j < 9; j++)
	{
		for (int k = 0; k < 4; k++)
		{
			qin[k] = ChiX[k][j];
		}
		dcm[0][0] = qin[0] * qin[0] + qin[1] * qin[1] - qin[2] * qin[2] - qin[3] * qin[3];
		dcm[0][1] = 2.0f * (qin[1] * qin[2] + qin[0] * qin[3]);
		dcm[0][2] = 2.0f * (qin[1] * qin[3] - qin[0] * qin[2]);

		dcm[1][0] = 2.0f * (qin[1] * qin[2] - qin[0] * qin[3]);
		dcm[1][1] = qin[0] * qin[0] - qin[1] * qin[1] + qin[2] * qin[2] - qin[3] * qin[3];
		dcm[1][2] = 2.0f * (qin[2] * qin[3] + qin[0] * qin[1]);

		dcm[2][0] = 2.0f * (qin[1] * qin[3] + qin[0] * qin[2]);
		dcm[2][1] = 2.0f * (qin[2] * qin[3] - qin[0] * qin[1]);
		dcm[2][2] = qin[0] * qin[0] - qin[1] * qin[1] - qin[2] * qin[2] + qin[3] * qin[3];


		for (int i = 0; i < 3; i++)
		{
			ChiY[i][j] = dcm[i][0] * mag[0] + dcm[i][1] * mag[1] + dcm[i][2] * mag[2];
		}
	}
}

void MatrixSubVector(float ChiY[][9], float *magPredict, int nRows)
{
	for (int i = 0; i < nRows; i++)
	{
		for (int j = 0; j < 9; j++)
		{
			ChiY[i][j] -= magPredict[i];
		}

	}
}

void MeasureCovMatrix(float measureMatrix[][9], float covMatrix[][3], float *wc, int nRows)
{
	float sum;
	for (int i = 0; i < nRows; i++)
		for (int j = 0; j < nRows; j++)
		{
			sum = 0;
			for (int k = 0; k < 9; k++)
			{
				sum += measureMatrix[i][k] * wc[k] * measureMatrix[j][k];
			}
			covMatrix[i][j] = sum;

			if (i == j)
			{
				covMatrix[i][j] += 0.00001f;
			}
		}
}

void GainUKF(float YXcov[4][3], float Ycov[3][3], float K[4][3])
{
	float YcovTmp[3][3];

	invertMatrix(Ycov, YcovTmp);

	multiplyMatrix(YXcov, 4, 3, YcovTmp, 3, 3, K);

}

void updateState(float q[4], float K[4][3], float mag_now[3], float mag_predict[3])
{
	float magSub[3];

	for (int i = 0; i < 3; i++)
	{
		magSub[i] = mag_now[i] - mag_predict[i];
	}

	float qTmp[4];

	memset(qTmp, 0, 4 * sizeof(float));

	for (int i = 0; i < 4; i++)
		for (int j = 0; j < 3; j++)
		{
			qTmp[i] += K[i][j] * magSub[j];
		}

	for (int i = 0; i < 4; i++)
	{
		q[i] += qTmp[i];
	}
}

void updataStateCov(float P[4][4], float Ycov[3][3], float K[4][3])
{

	float K_tmp[4][3];

	for (int i = 0; i < 4; i++)
	{
		for (int j = 0; j < 3; j++)
		{
			K_tmp[i][j] = K[i][0] * Ycov[0][j] + K[i][1] * Ycov[1][j] + K[i][2] * Ycov[2][j];

		}

	}

	for (int i = 0; i < 4; i++)
	{
		for (int j = 0; j < 4; j++)
		{
			P[i][j] -= K_tmp[i][0] * K[j][0] + K_tmp[i][1] * K[j][1] + K_tmp[i][2] * K[j][2];

		}

	}


}

void UpdateThetaMatrix(float theta[4][4], float *gyr, float dt)
{
	theta[0][0] = 1.0f;
	theta[1][1] = 1.0f;
	theta[2][2] = 1.0f;
	theta[3][3] = 1.0f;

	dt *= 0.5f;

	theta[0][1] = -gyr[0] * dt;
	theta[0][2] = -gyr[1] * dt;
	theta[0][3] = -gyr[2] * dt;

	theta[1][0] = gyr[0] * dt;
	theta[1][2] = gyr[2] * dt;
	theta[1][3] = -gyr[1] * dt;

	theta[2][0] = gyr[1] * dt;
	theta[2][1] = -gyr[2] * dt;
	theta[2][3] = gyr[0] * dt;

	theta[3][0] = gyr[2] * dt;
	theta[3][1] = gyr[1] * dt;
	theta[3][2] = -gyr[0] * dt;
}
/*
void UKF_para(int L, float alpha, float beta, float kappa, float *gamma, float *wm, float *wc)
{
	float lamda = alpha * alpha * (L + kappa) - L;
	*gamma = sqrt(L + lamda);

	for (int i = 0; i < 2 * L + 1; i++)
	{
		wm[i] = 0.5 / (L + lamda);
		wc[i] = 0.5 / (L + lamda);
	}

	wm[0] = lamda / (L + lamda);
	wc[0] = lamda / (L + lamda) + 1 - alpha * alpha + beta;
}
*/
void UKF_quat(float *quat, float P[4][4], float *gyr, float *mag_prev, float *mag_now, float gamma, float *wm, float *wc, int L, float dt)
{
	float mag[3];

	//为四元数更新做准备，减少运算量
	//UpdateThetaMatrix(theta, gyr, dt);

	//四元数归一
	//X_quat = X_quat / norm(X_quat);
	QuatNormalize(quat);

	//状态协方差矩阵分解，为sigma粒子产生作准备
	//Psr = gamma*chol(P)';


	chol(Psr, P, PsrTmp, L);

	for (int i = 0; i < 4; i++)
		for (int j = 0; j < 4; j++)
		{
			Psr[i][j] *= gamma;
		}

	//产生sigma粒子矩阵
	// ChiX = [X_quat, X_quat*ones(1,L)+Psr, X_quat*ones(1,L)-Psr];
	for (int i = 0; i < 4; i++)
	{
		ChiXtmp[i][0] = quat[i];
	}

	for (int j = 0; j < 4; j++)
	{
		for (int i = 0; i < 4; i++)
		{
			ChiXtmp[i][j + 1] = quat[i] + Psr[i][j];
			ChiXtmp[i][j + 5] = quat[i] - Psr[i][j];
		}
	}

	//sigma粒子预测更新
	for (int j = 0; j < 2 * L + 1; j++)
	{
		float sum = 0.0f;
		for (int i = 0; i < 4; i++)
		{
			//ChiX[i][j] = theta[i][0] * ChiXtmp[0][j] + theta[i][1] * ChiXtmp[1][j] + theta[i][2] * ChiXtmp[2][j] + theta[i][3] * ChiXtmp[3][j];
			ChiX[i][j] = ChiXtmp[i][j];
			sum += ChiX[i][j] * ChiX[i][j];
		}

		sum = 1 / sqrt(sum);

		for (int i = 0; i < 4; i++)
		{
			ChiX[i][j] *= sum;
		}
	};

	//复制Chix
	for (int i = 0; i < 4; i++)
		for (int j = 0; j < 9; j++)
		{
			ChiXtmp[i][j] = ChiX[i][j];
		}

	QuatPredict(ChiX, quat, wm, 9);

	StateCovarianceMatrix(P, ChiX, quat, wc, 2 * L + 1);

	quatXmag(ChiXtmp, mag_now, ChiY);

	MagPredict(ChiY, mag, wm, 2 * L + 1);

	MatrixSubVector(ChiY, mag, 3);

	StateMeasureCovariance(YXcov, ChiX, 4, ChiY, 3, wc, 9);

	MeasureCovMatrix(ChiY, ChiYcov, wc, 3);

	GainUKF(YXcov, ChiYcov, K);

	updateState(quat, K, mag_prev, mag);

	updataStateCov(P, ChiYcov, K);
}

/*
void UKF_quat(float *quat, float P[4][4], float *gyr, float *mag_prev, float *mag_now, float gamma, float *wm, float *wc, int L, float dt)
{
	float theta[4][4];
	float mag[3];

	//为四元数更新做准备，减少运算量
	UpdateThetaMatrix(theta, gyr, dt);

	//四元数归一
	//X_quat = X_quat / norm(X_quat);
	QuatNormalize(quat);


	//状态协方差矩阵分解，为sigma粒子产生作准备
	//Psr = gamma*chol(P)';

	chol(Psr, P, PsrTmp, L);


	for (int i = 0; i < 4; i++)
		for (int j = 0; j < 4; j++)
		{
			Psr[i][j] *= gamma;
		}

	//产生sigma粒子矩阵
	// ChiX = [X_quat, X_quat*ones(1,L)+Psr, X_quat*ones(1,L)-Psr];
	for (int i = 0; i < 4; i++)
	{
		ChiXtmp[i][0] = quat[i];
	}

	for (int j = 0; j < 4; j++)
	{
		for (int i = 0; i < 4; i++)
		{
			ChiXtmp[i][j + 1] = quat[i] + Psr[i][j];
			ChiXtmp[i][j + 5] = quat[i] - Psr[i][j];
		}
	}

	//sigma粒子预测更新
	for (int j = 0; j < 2 * L + 1; j++)
	{
		float sum = 0.0f;
		for (int i = 0; i < 4; i++)
		{
			ChiX[i][j] = theta[i][0] * ChiXtmp[0][j] + theta[i][1] * ChiXtmp[1][j] + theta[i][2] * ChiXtmp[2][j] + theta[i][3] * ChiXtmp[3][j];

			sum += ChiX[i][j] * ChiX[i][j];
		}

		sum = 1 / sqrt(sum);

		for (int i = 0; i < 4; i++)
		{
			ChiX[i][j] *= sum;
		}
	};

	//复制Chix
	for (int i = 0; i < 4; i++)
		for (int j = 0; j < 9; j++)
		{
			ChiXtmp[i][j] = ChiX[i][j];
		}

	QuatPredict(ChiX, quat, wm, 9);

	StateCovarianceMatrix(P, ChiX, quat, wc, 2 * L + 1);

	quatXmag(ChiXtmp, mag_prev, ChiY);

	MagPredict(ChiY, mag, wm, 2 * L + 1);

	MatrixSubVector(ChiY, mag, 3);

	StateMeasureCovariance(YXcov, ChiX, 4, ChiY, 3, wc, 9);

	MeasureCovMatrix(ChiY, ChiYcov, wc, 3);

	GainUKF(YXcov, ChiYcov, K);

	updateState(quat, K, mag_now, mag);

	updataStateCov(P, ChiYcov, K);
}
*/
/*
int main()
{
	int L = 4;
	float alpha = 0.01f;
	float beta = 2.0f;
	float kappa = 0.0f;
	float gamma ;
	float wm[9];
	float wc[9];

	UKF_para(L, alpha, beta, kappa, &gamma, wm, wc);

	std::cout << gamma << std::endl;

	for (int i = 0; i < 9; i++)
	{
		printf("%.20f ", wm[i]);
	}
	std::cout <<  std::endl;
	std::cout << std::endl;

	for (int i = 0; i < 9; i++)
	{
		printf("%.20f ", wc[i]);
	}
	std::cout << std::endl;
}
*/
/*
void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
{
	double *q;
	double *P;
	double *gyr;
	double *mag_prev;
	double *mag_now;
	double *gama;
	double *wm;
	double *wc;
	double *length;

	double *X_out;
	double *P_out;

	int Prows, Pcols;
	int Qrows, Qcols;
	int Mrows, Mcols;
	int Grows, Gcols;

	float P_quat[4][4];
	float quat[4];
	float Mag_prev[3];
	float Mag_now[3];
	float Gyr[3];
	float Gama;
	float Wm[9];
	float Wc[9];
	int L;

	q = mxGetPr(prhs[0]);
	Qrows = mxGetM(prhs[0]);
	Qcols = mxGetN(prhs[0]);

	for (int j = 0; j < Qrows; j++)
	{
		for (int i = 0; i < Qcols; i++)
		{
			quat[i*Qrows + j] = q[i*Qrows + j];
		}
	}


	P = mxGetPr(prhs[1]);
	Prows = mxGetM(prhs[1]);
	Pcols = mxGetN(prhs[1]);

	for (int j = 0; j < Prows; j++)
	{
		for (int i = 0; i < Pcols; i++)
		{
			P_quat[j][i] = P[i*Prows + j];
		}

	}

	gyr = mxGetPr(prhs[2]);
	Grows = mxGetM(prhs[2]);
	Gcols = mxGetN(prhs[2]);

	for (int j = 0; j < Grows; j++)
	{
		for (int i = 0; i < Gcols; i++)
		{
			Gyr[i*Grows + j] = gyr[i*Grows + j];
		}
	}


	mag_prev = mxGetPr(prhs[3]);
	Mrows = mxGetM(prhs[3]);
	Mcols = mxGetN(prhs[3]);

	for (int j = 0; j < Mrows; j++)
	{
		for (int i = 0; i < Mcols; i++)
		{
			Mag_prev[i*Mrows + j] = mag_prev[i*Mrows + j];
		}
	}

	mag_now = mxGetPr(prhs[4]);
	Mrows = mxGetM(prhs[4]);
	Mcols = mxGetN(prhs[4]);

	for (int j = 0; j < Mrows; j++)
	{
		for (int i = 0; i < Mcols; i++)
		{
			Mag_now[i*Mrows + j] = mag_now[i*Mrows + j];
		}
	}


	gama = mxGetPr(prhs[5]);
	Gama = *gama;


	wm = mxGetPr(prhs[6]);
	Mrows = mxGetM(prhs[6]);
	Mcols = mxGetN(prhs[6]);


	for (int j = 0; j < Mrows; j++)
	{
		for (int i = 0; i < Mcols; i++)
		{
			Wm[i*Mrows + j] = wm[i*Mrows + j];
		}
	}



	wc = mxGetPr(prhs[7]);
	Mrows = mxGetM(prhs[7]);
	Mcols = mxGetN(prhs[7]);

	for (int j = 0; j < Mrows; j++)
	{
		for (int i = 0; i < Mcols; i++)
		{
			Wc[i*Mrows + j] = wc[i*Mrows + j];
		}
	}


	length = mxGetPr(prhs[8]);
	L = (int)(*length);

	UKF_quat(quat, P_quat, Gyr, Mag_prev, Mag_now, Gama, Wm, Wc, L);

	plhs[0] = mxCreateDoubleMatrix(4, 1, mxREAL);
	X_out = mxGetPr(plhs[0]);

	plhs[1] = mxCreateDoubleMatrix(4, 4, mxREAL);
	P_out = mxGetPr(plhs[1]);

	for (int i = 0; i < 4; i++)
		for (int j = 0; j < 1; j++)
		{
			X_out[j * 4 + i] = quat[j * 4 + i];
		}
	for (int i = 0; i < 4; i++)
		for (int j = 0; j < 4; j++)
		{
			P_out[j * 4 + i] = P_quat[i][j];
		}
}*/