卡尔曼滤波矩阵求逆方法比较

最近因为项目原因需要用到卡尔曼滤波，但是仿真时间和采样时间发生冲突，导致项目无法进行下去，而仿真程序大部分时间耗在了卡尔曼滤波公式的求逆上，求逆公式如下：

                    （1）

所以我需要寻求一种快速的求逆方法。在仿真程序中我采用的是LU分解求解矩阵逆，具体的方法参考矩阵论里面的知识，求逆代码如下：

Matrix Matrix::inv()	
	{
		//将矩阵进行LU分解
		Matrix inv_matrix;	//定义一个矩阵保存矩阵逆的值
		Matrix L_matrix,U_matrix;	//L_matrix为分解后的下三角矩阵，U_matrix为分解后的上三角矩阵
		Matrix P_matrix;			//P为矩阵A的变换矩阵
		int n=row;			//保存矩阵的维数
		int flag=0;
		int flag_array[max_dim]={0};
		double matrix_num_copy[max_dim][max_dim];
		if(inv_matrix.matrix_num!=NULL)
		{
			for(int i=0;i<inv_matrix.row;i++)
			{
				if((inv_matrix.matrix_num[i]!=NULL))
				{
				delete []inv_matrix.matrix_num[i];
				inv_matrix.matrix_num[i]=NULL;
				}
			}
				delete []inv_matrix.matrix_num;
				inv_matrix.matrix_num=NULL;
		}
		//矩阵初始化行和列
		inv_matrix.row=row;
		inv_matrix.col=col;
		inv_matrix.matrix_num=new double*[inv_matrix.row];
		for(int i=0;i<inv_matrix.row;i++)
		{
			inv_matrix.matrix_num[i]=new double [inv_matrix.col];
		}
		if(L_matrix.matrix_num!=NULL)
		{
			for(int i=0;i<L_matrix.row;i++)
			{
				if((L_matrix.matrix_num[i]!=NULL))
				{
				delete []L_matrix.matrix_num[i];
				L_matrix.matrix_num[i]=NULL;
				}
			}
				delete []L_matrix.matrix_num;
				L_matrix.matrix_num=NULL;
		}
		L_matrix.row=row;
		L_matrix.col=col;
		L_matrix.matrix_num=new double*[L_matrix.row];
		for(int i=0;i<L_matrix.row;i++)
		{
			L_matrix.matrix_num[i]=new double [L_matrix.col];
		}
		if(U_matrix.matrix_num!=NULL)
		{
			for(int i=0;i<U_matrix.row;i++)
			{
				if((U_matrix.matrix_num[i]!=NULL))
				{
				delete []U_matrix.matrix_num[i];
				U_matrix.matrix_num[i]=NULL;
				}
			}
				delete []U_matrix.matrix_num;
				U_matrix.matrix_num=NULL;
		}
		U_matrix.row=row;
		U_matrix.col=col;
		U_matrix.matrix_num=new double*[U_matrix.row];
		for(int i=0;i<U_matrix.row;i++)
		{
			U_matrix.matrix_num[i]=new double [U_matrix.col];
		}
		if(P_matrix.matrix_num!=NULL)
		{
			for(int i=0;i<P_matrix.row;i++)
			{
				if((P_matrix.matrix_num[i]!=NULL))
				{
				delete []P_matrix.matrix_num[i];
				P_matrix.matrix_num[i]=NULL;
				}
			}
				delete []P_matrix.matrix_num;
				P_matrix.matrix_num=NULL;
		}
		P_matrix.row=row;
		P_matrix.col=col;
		P_matrix.matrix_num=new double*[P_matrix.row];
		for(int i=0;i<P_matrix.row;i++)
		{
			P_matrix.matrix_num[i]=new double [P_matrix.col];
		}
		//保存下矩阵A的数据，防止被改变
		for(int i=0;i<row;i++)
		{
			for(int j=0;j<col;j++)
			{
				matrix_num_copy[i][j]=matrix_num[i][j];
			}
		}
		for(int i=0;i<n;i++)		//初始化行数
		{
			flag_array[i]=i;
		}
		for(int k=0;k<n;k++)
		{
			double p=0;
			for(int i=k;i<n;i++)//找到每一列中的最大值，作为主对角线上的元素
			{
				if(abs(matrix_num_copy[i][k])>p)
				{
					p=abs(matrix_num_copy[i][k]);
					flag=i;	//将找到最大值的行数进行标记
				}
			}
			int keep1=0;
			if(p>0)
			{
				//进行矩阵行标记交换
				keep1=flag_array[k];
				flag_array[k]=flag_array[flag];
				flag_array[flag]=keep1;
			}
			else
			{
				std::cout<<"矩阵非奇异"<<std::endl;
				for(int i=0;i<inv_matrix.row;i++)
				{
					for(int j=0;j<inv_matrix.col;j++)
					{
						inv_matrix.matrix_num[i][j]=0;
					}
				}
				return inv_matrix;
			}
			double keep=0;
			for(int i=0;i<n;i++)
			{
				//对应矩阵行元素交换
				keep=matrix_num_copy[k][i];
				matrix_num_copy[k][i]=matrix_num_copy[flag][i];
				matrix_num_copy[flag][i]=keep;
			}
			for(int i=k+1;i<n;i++)
			{
				matrix_num_copy[i][k]=matrix_num_copy[i][k]/matrix_num_copy[k][k];
				for(int j=k+1;j<n;j++)
				{
					matrix_num_copy[i][j]=matrix_num_copy[i][j]-matrix_num_copy[i][k]*matrix_num_copy[k][j];
				}
			}
			
		}
		//得到LU矩阵
		for(int i=0;i<row;i++)
			{
				for(int j=0;j<=i;j++)
				{
					if(i==j)
					{
						L_matrix.matrix_num[i][j]=1;
					}
					else
					{
						L_matrix.matrix_num[i][j]=matrix_num_copy[i][j];
					}
				}
			}
		for(int i=0;i<row;i++)
		{
			for(int j=i;j<col;j++)
			{
					U_matrix.matrix_num[i][j]=matrix_num_copy[i][j];
			}
		}
		//通过flag_array数组得到变换矩阵P
		for(int i=0;i<row;i++)
		{
			for(int j=0;j<col;j++)
			{
					if(j==flag_array[i])
					{
						P_matrix.matrix_num[i][j]=1;
					}
					else
					{
						P_matrix.matrix_num[i][j]=0;
					}
			}
		}
		//利用LU分解求列向量LUA'=P,可以分解为LUx1=P1，A'为x相量的合并
		for(int i=0;i<n;i++)
		{	
			double y[max_dim]={0};		//解方程时的保留矩阵
			double x[max_dim]={0};
			//解向量y
			for(int j=0;j<n;j++)
			{	
				double result=0;
				for(int k=0;k<j;k++)
				{
					result+=L_matrix.matrix_num[j][k]*y[k];
				}
				y[j]=P_matrix.matrix_num[j][i]-result;
			}
			//解向量x
			for(int j=n-1;j>=0;j--)
			{
				double result=0;
				for(int k=j+1;k<n;k++)
				{
					result+=U_matrix.matrix_num[j][k]*x[k];
				}
				x[j]=(y[j]-result)/U_matrix.matrix_num[j][j];
				inv_matrix.matrix_num[j][i]=x[j];
			}
		}
		return inv_matrix;
	}

由于该方法无法满足我的需求，基于卡尔曼滤波求逆矩阵具有正定的性质，我在知网里面找到一篇文献是关于矩阵快速求逆的方法，然而经过测试，这种方法的效率还不如LU分解，白费了我一天时间测试该方法，所以这里不得不吐槽中国学者的学术素质，测试结果肯定造假。不管怎样还是得聊聊这种算法：

首先令

                                        ，

而且                                       

则有：                                      

                        （2）

如果矩阵的谱半径小于1，则：

                                                                        （3）

所以在满足一定精度的情况下矩阵求逆就只需要计算前i项就可以了。

然而这个算法存在一定的局限性，即矩阵谱半径必须小于1,对于大多数情况是不满足这种条件的，解决的办法就是首先找到矩阵Mk的模最大特征值然后对Nk进行如下变换：

                              （4）

经过上述变换之后就能保证矩阵Nk的谱半径小于1，最后收敛。但是该结论还有一个前提条件，就是矩阵Mk为正定或者负定，换句话说就是矩阵Mk的特征值都为正或者都为负，绝对不能出现符号不同的情况，如果出现符号不同，则Nk不收敛，

自然无法用该方法进行求解，而卡尔曼矩阵求逆矩阵都是正定的，所以该方法满足上述条件。

由上可知上述求逆的过程主要分为两步：1.求矩阵Mk的模最大特征值；2.利用公式2进行求解。

1.求矩阵Mk的模最大特征值

对于求解矩阵模最大特征值问题，可以参考《计算方法》。里面有一种叫做乘幂法进行计算，原理可以简单说一下：

（1）首先给定一个初始向量x0,该向量元素可以随便给定。

（2）对于给定的初始向量x0，假定矩阵M的n个特征向量分别为V1、V2、。。。。。Vn,由于特征向量之间线性无关，所以x0可以用n个特征向量进行表示：

                        （5）

对于矩阵M有：

                                    （6）

这里不妨假设为最大特征值，则有:

（7）

有：

                （8）

所以：

                                                                （9）

注意这里的向量相除是对应元素相除。由此便可以求出模最大绝对值。

在知道最大绝对值后，根据数据情况对其进行求解则能得到矩阵的逆，程序如下：

#include<iostream>
#include"Matrix.h"
#include<math.h>
#include<time.h>
using namespace std;
int main()
{
	FILE *init_file;
	double x_num[100][max_dim] = { 0 };
	for (int i = 0; i < 100; i++)
	{
		x_num[i][0] = i;
	}
	double A_num[100][max_dim] = { { 6,-2,2 },{ -2,5,0 },{ 2,0,7 } };
	if ((init_file = fopen("mm.txt", "r")) == NULL)
	{
		cout << "Cannot open this file\n" << endl;
	}
	//读取初始条件信息
	while (!feof(init_file))
	{
		for(int i=0;i<100;i++)
			for (int j = 0; j < 100; j++)
			{
				fscanf(init_file, "%lf ", &A_num[i][j]);
			}
	}
	Matrix x0(100, 1, x_num);
	Matrix A(100, 100, A_num);
	double error = 10;
	Matrix eye;
	eye.Matrix_I(100);
	Matrix x_last;
	double lamda = 0;
	double lamda_last = 0;
	clock_t start, finish;
	double totaltime;
	start = clock();
	for(int i=0;i<100;i++)
	{
		while (abs(error)>1)
		{
			lamda = 0;
			x_last = x0;
			x0 = A*x0;
			for (int i = 0; i < x0.put_row(); i++)
			{
				double a = x0.put_num()[i][0] / x_last.put_num()[i][0];
				if (abs(lamda) < abs(a))
				{
					lamda = a;
				}
			}
			error = lamda - lamda_last;
			lamda_last = lamda;
		}
		if (lamda > 1)
		{
			lamda += 1;
		}
		else if (lamda < -1)
		{
			lamda -= 1;
		}
		else
		{
			lamda = lamda;
		}
		Matrix N = A/lamda- eye;
		Matrix N2 = N*N;
		/*Matrix N4 = N2*N2;*/
		//Matrix A1 = (eye - N + N2 ) / lamda;//同样方法求逆需要0.385秒
		//A.inv();//15*15矩阵求逆1000次需要0.171秒
	}
	finish = clock();
	totaltime = (double)(finish - start) / CLOCKS_PER_SEC;
	cout << "\n此程序的运行时间为" << totaltime << "秒！" << endl;
	getchar();
	return 1;
}

上述代码因为用到了我自己写的一个矩阵类，所以你直接复制粘贴的话会出现问题，如果需要完整程序可以通过以下链接进行下载：

https://download.csdn.net/download/hxlove123456/10533576