reccate.cpp

迭代计算一个四阶黎卡提方程

/************************************************************/
/********************"系统仿真实验一——连续仿真"************************/
/********************"求解reccate方程"************************/
/************************************************************/
#include "math.h"
#include "iostream.h"
#include "STDIO.h"

double JudgeE(double P[4][4],double _P[4][4]);
double *SolutionP(double P[4][4],double T[4][4],double M[4][4]);
int cal_n=0;
//Function Name			:main
//Sub Functions used	:JudgeE(),SolutionP()
//Function Purpose		:solve reccate equation
void main()
{
	static double A[4][4]={
			{-8,-2.176,0,0},
			{97.173,-3.8092,-2.0954,0},
			{0,435.919,0,-871.838},
			{0,0,1,0}
		};
	static double B[4]={400,0,0,0};
	double Q[4][4]={
				{0,0,0,0},
				{0,0,0,0},
				{0,0,0,0},
				{0,0,0,0.2}
			};
	double R=1.0;
	double P[4][4]={
				{0,0,0,0},
				{0,0,0,0},
				{0,0,0,0},
				{0,0,0,0}
			};
	double _P[4][4]={
			{1,0,0,0},
			{0,0,0,0},
			{0,0,0,0},
			{0,0,0,0}
		};
	double T[4][4];
	double M[4][4];
/********************"计算初始的T和M"************************/
	for(int i=0;i<4;i++)
	{
		for(int j=0;j<4;j++)
		{
			T[i][j]=A[i][j];
			M[i][j]=-1.0*Q[i][j];
		}
	}
/**********************"迭代计算P值"*************************/
/*****************"P是Riccate代数方程的解"*******************/
/*****************"_P是上一步迭代的方程解"*******************/
	while(JudgeE(P,_P)>pow(0.1,15))
	{
		
		for(int i=0;i<4;i++)
		{
			for(int j=0;j<4;j++)
			{
				_P[i][j]=P[i][j];
			}
		}
		double *solution=SolutionP(P,T,M);
		for(i=0;i<4;i++)
		{
			for(int j=0;j<4;j++)
			{
				P[i][j]=solution[i*4+j];
			}
		}
/******************"重新计算T和M矩阵"***********************/
		for(i=0;i<4;i++)
		{
			for(int j=0;j<4;j++)
			{
				if(i==0)
				{
					T[i][j]=A[i][j]-160000*P[i][j];
				}
				else
					T[i][j]=A[i][j];
			}
		}
		for(i=0;i<4;i++)
		{
			for(int j=0;j<4;j++)
			{
				M[i][j]=-1*Q[i][j]-160000*P[i][0]*P[0][j];
			}
		}
		cout<<JudgeE(P,_P)<<endl;
	}
/*******************迭代完成，计算K矩阵"********************/
	FILE *pFile=fopen("result.txt","w");
	fprintf(pFile,"Riccate代数方程的解P：\n");
	for(i=0;i<4;i++)
	{
		for(int j=0;j<4;j++)
		{
			fprintf(pFile,"%f\t",P[i][j]);
			if(j==3)
				fprintf(pFile,"\n");
		}
	}
	fprintf(pFile,"全状态反馈系数阵K:\n");
	double K[4];
	for(i=0;i<4;i++)
	{
		K[i]=-400*P[0][i];
		fprintf(pFile,"%f\t",K[i]);
	}
	fclose(pFile);
	cout<<cal_n<<endl;
/**************************************************************************************/
}

//Function Name			:JudgeE
//Sub Functions used	:
//Function Purpose		:计算估计误差
double JudgeE(double P[4][4],double _P[4][4])
{
	double result=0;
	cal_n++;
	for(int i=0;i<4;i++)
	{
		for(int j=0;j<4;j++)
		{
			result=result+pow((P[i][j]-_P[i][j]),2);
		}
	}
	return result;
}

//Function Name			:SolutionP
//Sub Functions used	:
//Function Purpose		:求一步迭代的Riccate代数方程的解
double *SolutionP(double P[4][4],double T[4][4],double M[4][4])
{
	double *solution=new double[16];
	/*****************************"增广矩阵"*****************************/
	double Matrix[16][17]=
	{      11                  12                 13                14            21            22           23           24           31           32         33        34         41        42           43       44
	11	{2*T[0][0],	    	T[1][0],	    	T[2][0],    	T[3][0],    	T[1][0],    	0,	          0,	      0,   	    T[2][0],    	0,	      0,	     0,   	T[3][0],    	0,	       0,	    0,   	M[0][0]},
	12	{T[0][1],	    T[0][0]+T[1][1],	    T[2][1],    	T[3][1],     	  0,      	T[1][0],          0,    	  0,        	0,	    T[2][0],	  0,         0,       	0,   	T[3][0],       0,	    0,	    M[0][1]},
	13	{T[0][2],	       T[1][2],      	T[2][2]+T[0][0],	T[3][2],	      0,	        0,      	T[1][0],	  0,        	0,	        0,	 T[2][0],	     0,	        0,	        0,  	T[3][0],	0,	    M[0][2]},
	14	{T[0][3],	       T[1][3],	            T[2][3],	T[3][3]+T[0][0],	  0,	        0,	          0,	    T[1][0],        0,       	0,	      0,	  T[2][0],    	0,	        0,	       0,	 T[3][0],	M[0][3]},
	
	22	{0,	               T[0][1],	                0,	            0,	        T[0][1],  T[1][1]+T[1][1],	T[2][1],	T[3][1],    	0,	     T[2][1],	  0,	     0,	        0,	     T[3][1],	   0,    	0,   	M[1][1]},
	23	{0,                	0,               	T[0][1],        	0,      	T[0][2],	 T[1][2],	T[1][1]+T[2][2],T[3][2],    	0,      	0,	   T[2][1],	     0,	        0,	        0,	   T[3][1],	    0,	    M[1][2]},
	24	{0,	                0,	                    0,	        T[0][1],	    T[0][3],   	 T[1][3],	   T[2][3],	T[1][1]+T[3][3],	0,	        0,	      0,	  T[2][1],	    0,       	0,	       0,	 T[3][1],	M[1][3]},
	 
	33	{0,	                0,	                 T[0][2],        	0,	          0,	        0,	        T[1][2],	  0,        T[0][2],	 T[1][2],T[2][2]+T[2][2],T[3][2],	0,	        0,    	T[3][2],	0,	    M[2][2]},
	34	{0,              	0,	                    0,	        T[0][2],        	0,       	0,          	0,   	T[1][2],	T[0][3],    T[1][3],	T[2][3],T[2][2]+T[3][3],0,	        0,	       0,	 T[3][2],	M[2][3]},  

	44	{0,             	0,	                    0,	        T[0][3],	        0,	        0,	            0,   	T[1][3],	    0,	        0,        0,      T[2][3],	 T[0][3],   T[1][3],	T[2][3],T[3][3]+T[3][3],M[3][3]}
	};
	/******************************"主元素消去法求解方程"**************************/
	for(int i=0;i<15;i++)
	{
		for(int j=i+1;j<16;j++)
		{
			double saveJ=Matrix[j][i];
			for(int x=0;x<17;x++)
			{
				Matrix[j][x]=Matrix[j][x]-Matrix[i][x]*saveJ/Matrix[i][i];
			}
		}
	}
	/******************************"解对角阵"**************************************/
	for(i=15;i>=0;i--)
	{
		double temp=0;
		for(int j=i+1;j<16;j++)
		{
			temp=temp+solution[j]*Matrix[i][j];
		}
		solution[i]=(Matrix[i][16]-temp)/Matrix[i][i];
	}
	return solution;
}