C语言解算状态空间方程（基于四阶龙格库塔）

最新推荐文章于 2024-07-09 21:29:15 发布

boboelec

最新推荐文章于 2024-07-09 21:29:15 发布

阅读量418

点赞数 5

文章标签： c语言算法开发语言

本文链接：https://blog.csdn.net/boboelec/article/details/136766384

版权

本文详细描述了如何利用四阶龙格库塔法对线性系统进行状态更新，通过矩阵运算处理A、B、C和D矩阵，实现输入u的仿真并计算输出Y。

摘要由CSDN通过智能技术生成

#include <math.h>
#include <stdio.h>
#include <stdlib.h>
int main()
{
	printf("--------------------------------------------------------------------------\r\n");
	printf("--------------------------------------------------------------------------\r\n");
	printf("--------------------------------------------------------------------------\r\n");
	
	

	const double h = 0.001;//周期
	const int n = 4;//A矩阵的行数
	const int m = 1;//B矩阵的列数
	const int r = 3;//C矩阵的行数
	double k1[n];//k1的维数由A矩阵的行数决定
	double k2[n];//k2的维数由A矩阵的行数决定
	double k3[n];//k3的维数由A矩阵的行数决定
	double k4[n];//k4的维数由A矩阵的行数决定

	double X0_init = 0;//X0状态初始值，默认设置为0
	double A[n][n] = { 0, 1, 0, 0, -1, 0, 1, 0,0, 0, 0, 1, 1, 0, -1, 0 };//A矩阵定义
	double B[n][m] = {0,1,0,0};//B矩阵定义
	double C[r][n] = {1,0,0,0,0,0,1,0,0,0,1,0};//C矩阵定义
	double D[r][m] = {0,0,0};//D矩阵定义
	
	static double X0[n] = {0,0,0,0};//X0状态初始向量定义
	
	double u[m] = {0.1};//输入不定维数的U向量
	double u1[m];//计算K2时的中间变量
	double Ax0[n];//AX0乘积数组
	double Bu[n];//Bu的乘积
	double h_2_Ak1[n];//k2中的中间变量h/2Ak1
	double Bu1[n];//计算四阶龙格库塔k2中的B*(u + h / 2)
	double h_2_Ak2[n];//k3中的中间变量h/2Ak2
	double h_Ak3[n];//计算K4中的中间变量h*A*k3
	double u2[m];//计算K4时的中间变量
	double Bu2[n];//计算四阶龙格库塔k4中的B*(u + h)
	double Cx[r];//
	double Du[r];
	double Y[r];
	FILE* outFile;
	outFile = fopen("Data.txt", "w");

	for (int total = 0; total < 3000; total++) {
		//printf("++++++++++%d\r\n", total+1);
		//Ax0计算
		for (int i5 = 0; i5 < n; i5++)
		{
			for (int j5 = 0; j5 < 1; j5++)
			{
				double temp = 0.0;
				for (int k5 = 0; k5 < n; k5++)
				{
					temp += A[i5][k5] * X0[k5];
				}
				Ax0[i5] = temp;// 0 0 0 0
				//printf("Ax0:%lf\r\n", Ax0[i5]);
			}
		}

		//B*U乘积计算	
		for (int i6 = 0; i6 < n; i6++)
		{
			for (int j6 = 0; j6 < 1; j6++)
			{
				double temp1 = 0.0;
				for (int k6 = 0; k6 < m; k6++)
				{
					temp1 += B[i6][k6] * u[k6];
				}
				Bu[i6] = temp1;// 0 1 0 0
				//printf("Bu:%lf\r\n", Bu[i6]);
			}
		}
#
		//四阶龙格库塔计算K1
		for (int i7 = 0; i7 < n; i7++)
		{

			k1[i7] = Ax0[i7] + Bu[i7];// 0 1 0 0
			//printf("K1:%lf\r\n", k1[i7]);

		}

		//四阶龙格库塔中的k2中的(h/2)A(K1)
		for (int i8 = 0; i8 < n; i8++)
		{
			for (int j8 = 0; j8 < 1; j8++)
			{
				double temp2 = 0.0;
				for (int k8 = 0; k8 < n; k8++)
				{
					temp2 += A[i8][k8] * k1[k8];
				}
				h_2_Ak1[i8] = h / 2 * temp2;// 1 1 1 1
				//printf("h_2_Ak1:%lf\r\n", h_2_Ak1[i8]);
			}
		}

		//计算四阶龙格库塔k2中的(u+h/2)
		for (int i9 = 0; i9 < m; i9++)
		{

			u1[i9] = u[i9] + h / 2;// 2
			//printf("u1:%lf\r\n", u1[i9]);
		}

		//计算四阶龙格库塔k2中的B*(u+h/2)
		for (int i10 = 0; i10 < n; i10++)
		{
			for (int j10 = 0; j10 < 1; j10++)
			{
				double temp3 = 0.0;
				for (int k10 = 0; k10 < m; k10++)
				{
					temp3 += B[i10][k10] * u1[k10];
				}
				Bu1[i10] = temp3;// 0 2 0 0
				//printf("Bu1:%lf\r\n", Bu1[i10]);
			}
		}

		//计算四阶龙格库塔K2
		for (int i11 = 0; i11 < n; i11++)
		{
			k2[i11] = Ax0[i11] + h_2_Ak1[i11] + Bu1[i11];// 1 3 1 1
			//printf("K2:%lf\r\n", k2[i11]);
		}


		//首先计算k3中的(h/2)Ak2
		for (int i12 = 0; i12 < n; i12++)
		{
			for (int j12 = 0; j12 < 1; j12++)
			{
				double temp4 = 0.0;
				for (int k12 = 0; k12 < n; k12++)
				{
					temp4 += A[i12][k12] * k2[k12];
				}
				h_2_Ak2[i12] = h / 2 * temp4;// 6 6 6 6
				//printf("h_2_Ak2:%lf\r\n", h_2_Ak2[i12]);
			}
		}

		//计算龙格库塔K3
		for (int i13 = 0; i13 < n; i13++)
		{
			k3[i13] = Ax0[i13] + h_2_Ak2[i13] + Bu1[i13];// 6 8 6 6
			//printf("k3:%lf\r\n", k3[i13]);
		}

		//计算龙格库塔k4的h*A*k3
		for (int i14 = 0; i14 < n; i14++)
		{
			for (int j14 = 0; j14 < 1; j14++)
			{
				double temp5 = 0.0;
				for (int k14 = 0; k14 < n; k14++)
				{
					temp5 += A[i14][k14] * k3[k14];
				}
				h_Ak3[i14] = h * temp5;//52 52 52 52
				//printf("h_Ak3:%lf\r\n", h_Ak3[i14]);
			}
		}

		//计算龙格库塔K4中间变量u+h
		for (int i15 = 0; i15 < m; i15++)
		{
			u2[i15] = u[i15] + h;//3
			//printf("u2:%lf\r\n", u2[i15]);
		}

		//计算四阶龙格库塔k4中的B*(u+h)
		for (int i16 = 0; i16 < n; i16++)
		{
			for (int j16 = 0; j16 < 1; j16++)
			{
				double temp6 = 0.0;
				for (int k16 = 0; k16 < m; k16++)
				{
					temp6 += B[i16][k16] * u2[k16];
				}
				Bu2[i16] = temp6;// 0 3 0 0
				//printf("Bu2:%lf\r\n", Bu2[i16]);
			}
		}

		//计算四阶龙格库塔k4
		for (int i17 = 0; i17 < n; i17++)
		{

			k4[i17] = Ax0[i17] + h_Ak3[i17] + Bu2[i17];//52 55 52 52
			//printf("k4:%lf\r\n", k4[i17]);
		}

		//更新x0
		for (int i18 = 0; i18 < n; i18++)
		{
			X0[i18] = X0[i18] + h / 6 * (k1[i18] + 2 * k2[i18] + 2 * k3[i18] + k4[i18]);// 22 26 22 22
			//printf("X0:%lf\r\n", X0[i18]);
		}

		//计算y输出中间变量CX

		for (int i19 = 0; i19 < r; i19++)
		{
			for (int j19 = 0; j19 < 1; j19++)
			{
				double temp7 = 0.0;
				for (int k19 = 0; k19 < n; k19++)
				{
					temp7 += C[i19][k19] * X0[k19];

				}
				Cx[i19] = temp7;// 22 22 22
				//printf("Cx:%lf\r\n", temp7);
			}
		}

		//计算y输出中间变量du
		for (int i20 = 0; i20 < r; i20++)
		{
			for (int j20 = 0; j20 < 1; j20++)
			{
				double temp8 = 0.0;
				for (int k20 = 0; k20 < m; k20++)
				{
					temp8 += D[i20][k20] * u[k20];
				}
				Du[i20] = temp8;//0 0 0
				//printf("Du:%lf\r\n", Du[i20]);
			}
		}

		//计算y最终值
		for (int i21 = 1; i21 < 2; i21++)
		{
			Y[i21] = Cx[i21] + Du[i21];//22 22 22
			printf("%.16lf\r\n", Y[i21]);
			fprintf(outFile, "%.16lf\n", Y[i21]);
		}
	}
	fclose(outFile);


	
 
return 1;



}