非线性方程组求解

非线性方程组求解2020-11-04

牛顿法
求四阶方阵逆矩阵

拟牛顿法（解决Jacobi矩阵奇异的broyden方法）
带松弛因子的牛顿法（扩大初值的选取范围，见如下算法）

即便使用带松弛因子的牛顿法，选取复杂非线性方程组的初值仍旧很困难。

#include<fstream>
#include <iostream> //标准输入输出流 
#include<pcl/point_cloud.h>
#include <pcl/point_types.h> //PCL对各种格式的点的支持头文件 
#include <pcl/io/pcd_io.h> //PCL的PCD格式文件的输入输出头文件 

#include<pcl/visualization/pcl_visualizer.h>
#include <pcl/visualization/cloud_viewer.h>//点云查看窗口头文件 

#include <boost/random.hpp> 
#include <pcl/console/time.h> 

#include<vector>
#include<vtkAutoInit.h>
#include<pcl/features/moment_of_inertia_estimation.h>

#include<math.h>
#include<pcl/common/transforms.h>

#include <pcl/filters/statistical_outlier_removal.h>  //滤波相关
#include <pcl/common/common.h>  
//以上头文件与算法无关
#include<fstream>
#include<iostream>
#include<iomanip>
#include<cmath>
#include<cstdlib>
#define N 4 //非线性方程组中的变量个数及方程个数
const int N2 = N * N;//jacobi矩阵的元素个数
#define eps 0.000001//收敛精度
#define Max 1000//最大迭代次数
using namespace std;
double getD3(double m[3][3])
{
	//求三阶方阵的行列式
	double L;
	L = m[0][0] * (m[1][1] * m[2][2] - m[1][2] * m[2][1]) - m[0][1] * (m[1][0] * m[2][2] - m[1][2] * m[2][0]) + m[0][2] * (m[1][0] * m[2][1] - m[1][1] * m[2][0]);
	//cout << "行列式=" <<L<< endl;
	return L;
}
void MatrixInv4Order(double m[4][4], double mI[4][4])//求四阶矩阵的逆矩阵
{
	double m_yuzishi[4][4];
	double m_bs[4][4];
	double d = 0;
	for (int i = 0; i < 4; i++)//对的
	{
		for (int j = 0; j < 4; j++)
		{
			double r[3][3];
			for (int k = 0; k < 3; k++)
			{
				for (int l = 0; l < 3; l++)
				{
					if (k < i)
					{
						if (l < j)
						{
							r[k][l] = m[k][l];
							//cout << "m" << k << l << endl;
						}
						else
						{
							r[k][l] = m[k][l + 1];
							//cout << "m" << k << l + 1 << endl;
						}
					}
					else
					{
						if (l < j)
						{
							r[k][l] = m[k + 1][l];
							//cout << "m" << k + 1 << l << endl;
						}
						else
						{
							r[k][l] = m[k + 1][l + 1];
							//cout << "m" << k + 1 << l + 1 << endl;
						}
					}
				}
			}
			/*cout << "i+j=" << i << "+" << j << endl;
			for (int i = 0; i < 3; i++)
			{
				cout << r[i][0] << "\t" << r[i][1] << "\t" << r[i][2] << endl;
			}
			*/
			double M = 1;
			M = getD3(r);
			cout << "M=" << M << endl;
			m_yuzishi[i][j] = M*pow(-1, i + j);
			
		}//得到代数余子式
	}
	/*for (int i = 0; i < 4; i++)
	{
		cout << m_yuzishi[i][0] << "\t" << m_yuzishi[i][1] << "\t" << m_yuzishi[i][2] << "\t" << m_yuzishi[i][3] << endl;
	}*/
	for (int i = 0; i < 4; i++)
	{
		for (int j = 0; j < 4; j++)
		{
			m_bs[i][j] = m_yuzishi[j][i];
			//cout << "代数余子式伴随矩阵的第" << i << j << "个元素" << m_bs[i][j] << endl;
		}
	}//得到代数余子式矩阵的转置矩阵，即m的伴随矩阵
	for (int i = 0; i < 4; i++)
	{
		d += m[0][i] * m_yuzishi[0][i];
	}//得到m矩阵的行列式d
	cout << "计算第一行的每个位置上元素与对应的代数余子式的乘积和:" << d << endl;
	for (int i = 0; i < 4; i++)
	{
		for (int j = 0; j < 4; j++)
		{
			mI[i][j] = m_bs[i][j] / d;
			//cout << "逆矩阵的第" << i << j << "个元素" << mI[i][j] << endl;
		}
	}//伴随矩阵m_bs除以行列式d 得到m矩阵的逆矩阵mI
}
void main()
{
	/*
	int i, j, k, iter = 0;
	double A[4][4] = { {1,2,0,0},{3,4,0,0},{0,0,1,0},{0,0,0,1} };
	double AInv[4][4];
	MatrixInv4Order(A, AInv);//逆雅可比矩阵
	
	for (i = 0; i < 4; i++)
	{
		cout << AInv[i][0] << "\t" << AInv[i][1] <<"\t"<<AInv[i][2]<<"\t"<<AInv[i][3]<< endl;
	}
	*/
	double x0[N] = { 1,1,1,1}, x1[N], yf[N], es, esmax, jacobi[N][N],omege=1;
	double norm1=0, norm2=0;
	//double theta/2 = x0[0];
	//double x = x0[1];
	//double y = x0[2];
	//double z = x0[3];
	int i, j, k, iter = 0;
	while (iter < Max)
	{
		iter++;
		cout << "迭代次数=" << iter << endl;
		jacobi[0][0] = 2 * x0[0];
		jacobi[1][0] = 3 * x0[0] * x0[0];
		jacobi[2][0] = 10 * x0[0];
		jacobi[3][0] = 0;
		jacobi[0][1] = 1;
		jacobi[1][1] = 3;
		jacobi[2][1] = 18*x0[1];
		jacobi[3][1] = 2 * x0[1];
		jacobi[0][2] = 1;
		jacobi[1][2] = 2 * x0[2];
		jacobi[2][2] =9* x0[2]* x0[2];
		jacobi[3][2] = 2 * x0[2];
		jacobi[0][3] = 1;
		jacobi[1][3] = 18*x0[3]* x0[3];
		jacobi[2][3] = 1;
		jacobi[3][3] = 2 * x0[3];
		/*
		jacobi[0][0] = -64.62233 * x0[3] * x0[3] * 2 * cos(x0[0]) - 54.766956 * x0[1] * x0[3] * 2 * cos(x0[0]) - 52.638565 * x0[2] * x0[3] * 2 * cos(x0[0]);
		jacobi[1][0] = 8.41782 * x0[3] * x0[3] * 2 * cos(x0[0]) - 1.7859 * x0[1] * x0[3] * 2 * cos(x0[0]) - 6.3037 * x0[2] * x0[3] * 2 * cos(x0[0]);
		jacobi[2][0] = 0;
		jacobi[3][0] = 0;
		jacobi[0][1] = -54.766956 * (sin(x0[0]) * sin(x0[0])) * x0[3];
		jacobi[1][1] = -1.7859 * (sin(x0[0]) * sin(x0[0])) * x0[3];
		jacobi[2][1] = 52.9810635;
		jacobi[3][1] = 2 * x0[1];
		jacobi[0][2] = -52.638565 * (sin(x0[0]) * sin(x0[0])) * x0[3];
		jacobi[1][2] = -6.3037 * (sin(x0[0]) * sin(x0[0])) * x0[3];
		jacobi[2][2] = 46.3348693;
		jacobi[3][2] = 2 * x0[2];
		jacobi[0][3] = -64.62233 * (sin(x0[0]) * sin(x0[0])) * 2 * x0[3] - 54.766956 * (sin(x0[0]) * sin(x0[0])) * x0[1] - 52.638565 * (sin(x0[0]) * sin(x0[0])) * x0[2];
		jacobi[1][3] = 8.4178181 * (sin(x0[0]) * sin(x0[0])) * 2 * x0[3] - 1.7858925 * (sin(x0[0]) * sin(x0[0])) * x0[1] - 6.3036967 * (sin(x0[0]) * sin(x0[0])) * x0[2];
		jacobi[2][3] = 73.6401481;
		jacobi[3][3] = 2 * x0[3];
		*/
		cout << "雅可比矩阵" << endl;
		for (i = 0; i < N; i++)
		{
			cout << jacobi[i][0] << "\t" << jacobi[i][1] << "\t" << jacobi[i][2] << "\t" << jacobi[i][3] << endl;
		}
		double jacobiInv[N][N];
		double c=0;
		MatrixInv4Order(jacobi, jacobiInv);//逆雅可比矩阵
		for (i = 0; i < N; i++)
		{
			cout << jacobiInv[i][0] << "\t" << jacobiInv[i][1] << "\t" << jacobiInv[i][2] << "\t" << jacobiInv[i][3] << endl;
		}
		//iterating process
		/*yf[0] = -(2273696103967291 * sin(x0[0]) * sin(x0[0]) * x0[3] * x0[3]) / 35184372088832 - (7707763832306761 * sin(x0[0]) * sin(x0[0]) * x0[1] * x0[3]) / 140737488355328 - (1852054857182169 * sin(x0[0]) * sin(x0[0]) * x0[2] * x0[3]) / 35184372088832 - 26.325336;
		yf[1] = (4738810307304077 * sin(x0[0]) * sin(x0[0]) * x0[3] * x0[3]) / 562949953421312 - (2010736199380941 * sin(x0[0]) * sin(x0[0]) * x0[1] * x0[3]) / 1125899906842624 - (7097330401394249 * sin(x0[0]) * sin(x0[0]) * x0[2] * x0[3]) / 1125899906842624 + 31.502981;
		yf[2] = (7456421807384143 * x0[1]) / 140737488355328 + (6521053128554395 * x0[2]) / 140737488355328 + (642467937043449 * x0[3]) / 8796093022208;
		yf[3] = x0[1] * x0[1] + x0[2] * x0[2] + x0[3] * x0[3] - 1;
		*/
		yf[0] = x0[0] * x0[0] + x0[1] + x0[2] + x0[3]-3;
		yf[1] = x0[0] * x0[0] * x0[0] + 3 * x0[1] + x0[2] * x0[2] + 6 * x0[3] * x0[3] * x0[3]-11;
		yf[2] = 5 * x0[0] * x0[0] + 9 * x0[1] * x0[1] + 3 * x0[2] * x0[2] * x0[2] + x0[3]-18;
		yf[3] = x0[1] * x0[1] + x0[2] * x0[2] + x0[3] * x0[3] - 3;
		cout << "yf为：" << endl;
		for (i = 0; i < N; i++)
		{
			cout << yf[i] << endl;
		}
		for (i = 0; i < N; i++)
		{
			for (j = 0; j < N; j++)
			{
				double jacobiInv2[N];
				jacobiInv2[j] = jacobiInv[i][j];
				c += jacobiInv2[j] * yf[j];
			}
			norm1 += yf[i] * yf[i];
			norm2 += (yf[i] - omege * c) * (yf[i] - omege * c);
			if (norm1 < norm2)
			{
				omege = 0.5;
			}
			else
			{
				omege = 1;
			}
			x1[i] = x0[i] - omege * c;
			//牛顿迭代
		}
		//得到x（k+1）
		for (i = 0; i < N; i++)
		{
			cout << "x(k+1)为："<<x1[i] << endl;
		}
		esmax = 0.0;
		for (i = 0; i < N; i++)
		{
			es = x1[i] - x0[i];

			if (fabs(es) > fabs(esmax))
			{
				esmax = es;
			}
		}
			cout <<"esmax为："<< esmax << endl;
			if (fabs(esmax) < eps)
			{
				break;
			}
			for (i = 0; i < N; i++)
			{
				x0[i] = x1[i];
			}
	}
	return;
}
//牛顿迭代法解三元二次方程组（C++版） - 百度文库 
//https://wenku.baidu.com/view/dcc133fd7c1cfad6195fa730.html

如果不带松弛因子，令omege一直等于1就可以