坐标系转换

坐标系转换

载体坐标系转换到NED

把载体坐标下的罗盘数据转换到地理坐标下

float X[3][3]; //必须由姿态解算算出该矩阵
void a2w_3d_trans(float a[XYZ],float w[XYZ])
{
		for(u8 i = 0;i<3;i++)
		{
			float temp = 0;
			for(u8 j = 0;j<3;j++)
			{	
				temp += a[j] *X[i][j]; //罗盘的数据乘以四元数解算的矩阵
			}
			w[i] = temp;
		}
}

//NED平面XY转平面航向坐标XY
void w2h_2d_trans(float w[XYZ],float ref_ax[XYZ],float h[XYZ])
{
	h[X] =  w[X] *  ref_ax[X]  + w[Y] *ref_ax[Y];
	h[Y] =  w[X] *(-ref_ax[Y]) + w[Y] *ref_ax[X];
	
}

//平面航向坐标XY转NED平面XY
void h2w_2d_trans(float h[XYZ],float ref_ax[XYZ],float w[XYZ])
{
	w[X] = h[X] *ref_ax[X] + h[Y] *(-ref_ax[Y]);
	w[Y] = h[X] *ref_ax[Y] + h[Y] *  ref_ax[X];
}