【GNSS】简单的矩阵库实现

矩阵运算

声明

/*------------------------------------------矩阵运算部分;-----------------------------------------------*/
/*矢量相乘;*/
bool VecPlus(const int n1, const double Vec1[], const int n2, const double Vec2[], double *result);
/*矢量相加;*/
bool VecAdd(const int n1, const double Vec1[], const int n2, const double Vec2[], double result[]);
/*矢量相减;*/
bool VecSub(const int n1, const double Vec1[], const int n2, const double Vec2[], double result[]);
/*矢量叉积;*/
bool VecCrossPlus(const int n1, const double Vec1[], const int n2, const double Vec2[], double result[]);
/*矩阵初始化;*/
bool MatInit(const int row, const int col, double mat[]);
/*矩阵加法;*/
bool MatAdd(const int row1, const int col1, const int row2, const int col2, const double A[], const double B[], double output[]);//矩阵相加;
/*矩阵减法;*/
bool MatSub(const int row1, const int col1, const int row2, const int col2, const double A[], const double B[], double output[]);//矩阵相减;
/*三种矩阵乘法，分别是数量乘法，普通乘法，点乘;*/
bool MatMul_Num(const double k, const int row, const int col, const double A[], double output[]);//矩阵相乘;
bool MatMul_Mat(const int row1, const int col1, const int row2, const int col2, const double A[], const double B[], double output[]);
bool MatMul_Poi(const int row1, const int col1, const int row2, const int col2, const double A[], const double B[], double output[]);
/*矩阵转置;*/
bool MatTrans(const int row, const int col, const double A[], double output[]);//矩阵转置;
/*求矩阵行列式;*/
double Calculate_Det(const double arr[], const int n);
/*矩阵求逆;*/
bool MatInver(int n, double a[], double b[]);//矩阵求逆;
/*计算BQBT;*/
bool MatBTQB(const int row1, const int col1, const int row2, const int col2, const double B[], const double Q[], double output[]);//求BQBT;
bool MatEye(int n, double eye[]);
/*输出矩阵;*/
void PrintfMat(const int row, const int col, const double Mat[]);

实现

/*----------------------------------------------------
矩阵运算模块;
包括;
	矩阵初始化：Matinit;
	矩阵相加：MatAdd;
	矩阵相减:MatSub;
	矩阵数量乘法：MatMul_Num;
	矩阵乘法：MatMul_Mat;
	矩阵点乘:MatMul_Poi;
	矩阵求逆：MatInver;
	矩阵常用BQBT计算：MatBQBT;
	矩阵行列式det计算：Calculate_Det;
	矩阵输出:PrintfMat;


-----------------------------------------------------*/

/*矢量相乘;*/
bool VecPlus(const int n1, const double Vec1[], const int n2, const double Vec2[], double *result)
{
	int i =0 ;
	*result = 0;
	if (n1!=n2)
	{
		printf("Vector's dimensions must agree\n");
		return false;
	}
	for (i; i < n1;i++)
	{
		*result = Vec2[i] * Vec1[i] + *result;
	}
	return true;
}

/*矢量相加;*/
bool VecAdd(const int n1, const double Vec1[], const int n2, const double Vec2[], double result[])
{
	int i = 0;
	if (n1!=n2)
	{
		printf("Vector's dimensions must agree\n");
		return false;
	}
	for (i; i < n1;i++)
	{
		result[i] = Vec1[i] + Vec2[i];
	}
	return true;
}

/*矢量相减;*/
bool VecSub(const int n1, const double Vec1[], const int n2, const double Vec2[], double result[])
{
	int i = 0;
	if (n1 != n2)
	{
		printf("Vector's dimensions must agree\n");
		return false;
	}
	for (i; i < n1; i++)
	{
		result[i] = Vec1[i] - Vec2[i];
	}
	return true;
}

/*矢量叉积;*/
bool VecCrossPlus(const int n1, const double Vec1[], const int n2, const double Vec2[], double result[])
{
	int i = 0;
	if (n1 != n2)
	{
		printf("Vector's dimensions must agree\n");
		return false;
	}
	result[0] = Vec1[1] * Vec2[2] - Vec2[1] * Vec1[2];
	result[1] = Vec1[2] * Vec2[0] - Vec1[0] * Vec2[2];
	result[2] = Vec1[0] * Vec2[1] - Vec2[0] * Vec1[1];
	return true;
}

/*-----------------------初始化矩阵;----------------------
//函数功能：初始化矩阵，将矩阵元素全部置0;
//输入参数：初始化矩阵 mat[]，矩阵行数 row，矩阵列数 col;
//输出参数：无;
//原理：;
----------------------------------------------------*/
bool MatInit(const int row,const int col,double mat[] )
{
	if (row<0||col<0)
	{
		printf("Row and Col must be 0+\n");
		return false;
	}
	for (int i = 0; i < row*col;i++)
	{
		mat[i] = 0;
	}
	return true;
}

/*-----------------------矩阵相加;----------------------
//函数功能：将两个矩阵相加，并输出相加结果;
//output=A+B;
//输入参数：相加矩阵 A[] B[] ,矩阵 A[] B[]的行数 row，列数 col，输出结果 output[];
//输出参数：output[];
//原理：两个维数相同的矩阵相加，其结果是矩阵对应位相加，本函数要求矩阵A与矩阵B维数相同，在函数内部无法判断维数是否冲突;
----------------------------------------------------*/
bool MatAdd(const int row1, const int col1, const int row2, const int col2, const double A[], const double B[], double output[])//矩阵相加;
{
	int i = 0;
	if (row1 != row2 || col1 != col2)
	{
		printf("MatAdd:Inner matrix dimensions must agree.\n");
		return false;
	}
	else
	{
		for ( i = 0; i < row1*col1; i++)
		{
			output[i] = A[i] + B[i];
		}
		return true;
	}
}

/*-----------------------矩阵相减;----------------------
//函数功能：将两个矩阵相减，并输出相减结果;
//output=A-B;
//输入参数：相减矩阵 A[] B[] ,矩阵 A[] B[]的行数 row，列数 col，输出结果 output[];
//输出参数：output[];
//原理：两个维数相同的矩阵相减，其结果是矩阵对应位相减，本函数要求矩阵A与矩阵B维数相同，在函数内部无法判断维数是否冲突;
--------------------------------------------------------*/
bool MatSub(const int row1, const int col1, const int row2, const int col2, const double A[], const double B[], double output[])//矩阵相减
{
	int i = 0;
	if (row1 != row2 || col1 != col2)
	{
		printf("MatAdd:Inner matrix dimensions must agree.\n");
		return false;
	}
	else
	{
		for (int i = 0; i < row1*col1; i++)
		{
			output[i] = A[i] - B[i];
		}
		return true;
	}
}

/*-----------------------矩阵的数量乘法;----------------------
//函数功能：将一个常数与一个矩阵相乘，并输出相乘结果;
//output= k * A;
//输入参数：常数 k;矩阵 A; 矩阵的行数 row;矩阵的列数 col;存储计算结果的矩阵 output;
//输出参数：output[];
//原理：矩阵运算法则;
----------------------------------------------------------*/
bool MatMul_Num(const double k, const int row, const int col,const double A[], double output[])//矩阵相乘
{
	int i = 0;
	for ( i = 0; i < row*col;i++)
	{
		output[i] = k*A[i];
	}
	return true;
}

/*-----------------------矩阵的乘法;----------------------
//函数功能：将一个矩阵与一个矩阵相乘，并输出相乘结果;
//output= A*B;
//输入参数：矩阵 A;矩阵 B; 矩阵的行数 row;矩阵的列数 col;output的列数 k,存储计算结果的矩阵 output;
//输出参数：output[];
//原理：矩阵运算法则;两个矩阵必须维数对应，矩阵A 的列数目等于矩阵B的行数，函数内部无法判断维数是否对齐;
----------------------------------------------------------*/
bool MatMul_Mat(const int row1, const int col1, const int row2, const int col2, const double A[], const double B[], double output[])
{
	int i, j, l;
	if (col1!=row2)
	{
		printf("MatAdd:Inner matrix dimensions must agree.\n");
		return false;
	}
	else
	{

		int u = 0;
		for ( i = 0; i <= row1 - 1; i++)
		for ( j = 0; j <= col2 - 1; j++)
		{
			u = i*col2 + j;
			output[u] = 0.0;
			for ( l = 0; l <= col1 - 1; l++)
			{
				output[u] = output[u] + A[i*col1 + l] * B[l*col2 + j];
			}
		}
		return true;
	}
}

/*-----------------------矩阵的点乘;----------------------*/
//函数功能：将一个矩阵与一个矩阵点乘（对应相乘），并输出相乘结果;
//output= A.*B;
//输入参数：矩阵 A;矩阵 B; 矩阵的行数 row;矩阵的列数 col;存储计算结果的矩阵 output;
//输出参数：output[];
//原理：矩阵运算法则;两个矩阵必须维数对应，矩阵A 的列数目等于矩阵B的行数，函数内部无法判断维数是否对齐;
/*----------------------------------------------------------*/
bool MatMul_Poi(const int row1, const int col1, const int row2, const int col2, const double A[], const double B[], double output[])
{
	int i;
	if (row1!=row2||col1!=col2)
	{
		printf("MatAdd:Inner matrix dimensions must agree.\n");
		return false;
	}
	else
	{
		for (i = 0; i < row1*col1; i++)
		{
			output[i] = A[i] * B[i];
		}
		return ture;
	}
}

/*-----------------------矩阵的转置;----------------------*/
//函数功能：求一个矩阵的转置矩阵;
//output= AT;
//输入参数：矩阵 A;矩阵的行数 row;矩阵的列数 col;存储转置结果的矩阵 output;
//输出参数：output[];
//原理：矩阵运算法则;
/*----------------------------------------------------------*/
bool MatTrans(const int row, const int col, const double A[], double output[])//矩阵转置
{

	for (int i = 0; i < row; i++)
	{
		for (int j = 0; j < col;j++)
		{
			output[j*row + i] = A[i*col + j];
		}
	}
	return true;
}

/*-----------------------矩阵的求逆;----------------------*/
//函数功能：求一个矩阵的转置矩阵;
//output= a^-1;
//输入参数：矩阵 a;矩阵的行数 n;存储求逆结果的矩阵 b;
//输出参数：b;
//原理：矩阵运算法则;矩阵A中不能有为0的元素
/*----------------------------------------------------------*/
bool MatInver(int n, double a[], double b[])
{
	int i, j, k, l, u, v, is[10], js[10];   /* matrix dimension <= 10 */
	double d, p;

	if (n <= 0)
	{
		printf("Error dimension in MatrixInv!\n");
		return false;
	}

	/* 将输入矩阵赋值给输出矩阵b，下面对b矩阵求逆，a矩阵不变; */
	for (i = 0; i < n; i++)
	{
		for (j = 0; j < n; j++)
		{
			b[i*n + j] = a[i*n + j];
		}
	}

	for (k = 0; k < n; k++)
	{
		d = 0.0;
		for (i = k; i < n; i++)   /* 查找右下角方阵中主元素的位置 ;*/
		{
			for (j = k; j<n; j++)
			{
				l = n*i + j;
				p = fabs(b[l]);
				if (p>d)
				{
					d = p;
					is[k] = i;
					js[k] = j;
				}
			}
		}

		if (d < 2.2204460492503131e-16)   /* 主元素接近于0，矩阵不可逆 */
		{
			printf("Divided by 0 in MatrixInv!\n");
			return false;
		}

		if (is[k] != k)  /* 对主元素所在的行与右下角方阵的首行进行调换; */
		{
			for (j = 0; j < n; j++)
			{
				u = k*n + j;
				v = is[k] * n + j;
				p = b[u];
				b[u] = b[v];
				b[v] = p;
			}
		}

		if (js[k] != k)  /* 对主元素所在的列与右下角方阵的首列进行调换; */
		{
			for (i = 0; i < n; i++)
			{
				u = i*n + k;
				v = i*n + js[k];
				p = b[u];
				b[u] = b[v];
				b[v] = p;
			}
		}

		l = k*n + k;
		b[l] = 1.0 / b[l];  /* 初等行变换; */
		for (j = 0; j < n; j++)
		{
			if (j != k)
			{
				u = k*n + j;
				b[u] = b[u] * b[l];
			}
		}
		for (i = 0; i < n; i++)
		{
			if (i != k)
			{
				for (j = 0; j < n; j++)
				{
					if (j != k)
					{
						u = i*n + j;
						b[u] = b[u] - b[i*n + k] * b[k*n + j];
					}
				}
			}
		}
		for (i = 0; i < n; i++)
		{
			if (i != k)
			{
				u = i*n + k;
				b[u] = -b[u] * b[l];
			}
		}
	}

	for (k = n - 1; k >= 0; k--)  /* 将上面的行列调换重新恢复; */
	{
		if (js[k] != k)
		{
			for (j = 0; j < n; j++)
			{
				u = k*n + j;
				v = js[k] * n + j;
				p = b[u];
				b[u] = b[v];
				b[v] = p;
			}
		}
		if (is[k] != k)
		{
			for (i = 0; i < n; i++)
			{
				u = i*n + k;
				v = is[k] + i*n;
				p = b[u];
				b[u] = b[v];
				b[v] = p;
			}
		}
	}

	return true;
}
/*--------------------求一个方阵的行列式值;--------------------------;
函数名称：Calculate_Det
函数功能：求一个方阵的行列式值;
函数原理：线性代数运算规则，将线性代数课本;
函数要求：必须是方阵;
输入参数：;
	arr[]：矩阵元素;
	n：矩阵维数;
输出参数：;
	value：行列式运算结果;
-----------------------------------------------------------------*/
double Calculate_Det(const double arr[],const int n)
{
	double bak[100];
	int i, j, k, c;
	double sum = 0;

	if (n == 1)
	{
		return arr[0];
	}

	for (i = 0; i < n; i++)             /* 处理第1行的每一列元素; */
	{
		for (j = 1; j < n; j++)         /* 逐行处理; */
		{
			for (c = 0, k = 0; k < n; k++) /* 逐列处理; */
			{
				if (k == i)
				{
					continue;
				}
				bak[(j - 1)*(n-1)+c] = arr[j*n+k];
				c++;
			}
		}
		double a = arr[i];
		//double b = Calculate_Det(bak, n - 1);
		/* 计算arr(row, col) * D(row, col)的值 */
		double tempt = sum;
		sum += (i % 2 == 0 ? 1 : -1) * arr[i] * Calculate_Det(bak, n - 1);
	}
	return sum;
}

/*----------------------------最小二乘估计常用BQBT计算-------------------------
函数名称：MatBQBT;
函数功能：计算B*Q*BT 矩阵运算结果;
函数原理：线性代数运算法则;
函数要求：要求B，Q，BT矩阵维数对应;
输入参数：B,Q,BT矩阵元素以及B矩阵，Q矩阵的维数;
输出参数：运算结果output;
-------------------------------------------------------------------------------*/
bool MatBTQB(const int row1, const int col1, const int row2, const int col2, const double B[], const double Q[],double output[])//求BQBT;
{
	int i = 0;
	double BT[50 * 50];
	MatTrans(32, 4, B, BT);
	double BTQ[50 * 50];
	MatMul_Mat(col1, row1, row2, col2, BT, Q, BTQ);
	double BTQB[50 * 50];
	MatMul_Mat(col1, col2, row1, col1, BTQ, B, BTQB);
	for (i = 0; i < col1*col1;i++)
	{
		output[i] = BTQB[i];
	}
	return true;
}

bool MatEye(int n, double eye[])
{
	int i = 0;
	MatInit(n, n, eye);
	for (i = 0; i < n; i++)
	{
		eye[i*n + i ] = 1;
	}
	return true;
}
void PrintfMat( const int row, const int col,const double Mat[])
{
	printf("\n");
	for (int i = 0; i < row*col;i++)
	{
		printf("%lf ", Mat[i]);
		if ((i + 1) % col == 0)
		{printf("\n");}
	}
}