开发: C++知识库 Java知识库 JavaScript Python PHP知识库人工智能区块链大数据移动开发嵌入式开发工具数据结构与算法开发测试游戏开发网络协议系统运维
教程: HTML教程 CSS教程 JavaScript教程 Go语言教程 JQuery教程 VUE教程 VUE3教程 Bootstrap教程 SQL数据库教程 C语言教程 C++教程 Java教程 Python教程 Python3教程 C#教程
数码: 电脑笔记本显卡显示器固态硬盘硬盘耳机手机 iphone vivo oppo 小米华为单反装机图拉丁

-> 数据结构与算法 -> 二次规划求解，c++代码实现（Eigen库） -> 正文阅读

[数据结构与算法]二次规划求解，c++代码实现（Eigen库）

代码用到c++ Eigen矩阵运算库，官网下载，添加附加包含目录，附加库目录。

一、等式线性约束的二次规划

拉格朗日法求解

$\bar{x}$ 即为方程组的解

拉格朗日法c++代码

template<typename Derived>
void qp_lagrange(Eigen::MatrixBase<Derived>& H, Eigen::MatrixBase<Derived>& c, Eigen::MatrixBase<Derived>& A, Eigen::MatrixBase<Derived>& b,
	Eigen::MatrixBase<Derived>& x, Eigen::MatrixBase<Derived>& lambda, const int& dim, const int& m) {
	Eigen::MatrixXd G(dim, dim);
	Eigen::MatrixXd B(m, dim);
	Eigen::MatrixXd C(m, m);

	G = H.inverse() - H.inverse() * A.transpose() * (A * H.inverse() * A.transpose()).inverse() * A * H.inverse();
	B = (A * H.inverse() * A.transpose()).inverse() * A * H.inverse();
	C = -1 * (A * H.inverse() * A.transpose()).inverse();

	x = B.transpose() * b - G * c;
	lambda = B * c - C * b;
}

二、不等式线性约束二次规划问题

有两种方法求解：1）有效集法（active-set method）和2）光滑牛顿法（Smoothing Newton Method）。有效集法算法简单，但是依赖初始解必须满足边界条件；光滑牛顿法不依赖初始解，但是求解复杂。

1）有效集法

不等式约束取到等号，则该约束被激活，加入有效集。求解等式线性约束二次规划的子问题，得到解的迭代方向d和迭代步长 $\alpha$ ，对原问题的解进行迭代。

step0 给定初始解

step1 确定有效集

step2 求解子问题，得到子问题的 $d$ 和 $\lambda$

判断 $d$ 的模是否小于0

是：

??? 找最小的 $\lambda$ ，判断是否小于0

??? 小于0，停算，已找到最优解

??? 大于0，从有效集中，将该 $\lambda$ 对应的约束剔除，返回step2

否：

??? 求步长（求补偿所用到的约束在“无效集”中）

???

? ? $\alpha_{k}$ 大于1：

??????? 是：

??????? 步长取1，有效集不变

??????? 否：

??????? 步长取 $\alpha_{k}$ ，并把 $\alpha_{k}$ 对应的约束加入有效集

??? 返回step2

?有效集法代码：

template<typename Derived>
void qp_activeset(Eigen::MatrixBase<Derived>& H, Eigen::MatrixBase<Derived>& c, Eigen::MatrixBase<Derived>& Ae, Eigen::MatrixBase<Derived>& be,
	Eigen::MatrixBase<Derived>& Ai, Eigen::MatrixBase<Derived>& bi, const int& ne, const int& ni, Eigen::MatrixBase<Derived>& x) {
	double epsilon = 1e-9;
	Eigen::MatrixXd Sk = Eigen::MatrixXd::Zero(ni, 1);
	std::vector<double> asIndex;	//记录第几个不等式约束被选为有效集

	Eigen::MatrixXd bi_left(Ai.rows(), x.cols());
	bi_left = Ai * x;
	Eigen::MatrixXd Aee(1, 1);

	#pragma region set initial active set
	for (int i = 0; i < ni; i++) {
		double bb = bi(i, 0);
		if (bi_left(i, 0) <= bi(i, 0) + epsilon) {
			Sk(i, 0) = 1;
		}
	}
	#pragma endregion

	#pragma region main loop
	int iterTimes = 0;
	while (iterTimes < 150) {
		int activeSetSize = 0;
		for (int i = 0; i < Sk.rows(); i++) {
			if (Sk(i) == 1) {
				activeSetSize += 1;
			}
		}
		if (ne != 0) {
			Aee.resize(ne + activeSetSize, Ae.cols());
			Aee.block(0, 0, Ae.rows(), Ae.cols()) = Ae;
			int aee_index = 0;
			for (int i = 0; i < ni; i++) {
				if (Sk(i, 0) == 1) {
					Aee.block(ne + aee_index, 0, 1, Ae.cols()) = Ai.row(i);
					asIndex.push_back(i);
					aee_index += 1;
				}
			}
		}
		else {
			Aee.resize(activeSetSize, Ai.cols());
			int aee_index = 0;
			for (int i = 0; i < ni; i++) {
				if (Sk(i, 0) == 1) {
					Aee.block(aee_index, 0, 1, Ai.cols()) = Ai.row(i);
					asIndex.push_back(i);
					aee_index += 1;
				}
			}
		}

		// 解子问题
		Eigen::MatrixXd d = Eigen::MatrixXd::Zero(x.rows(), 1);
		Eigen::MatrixXd lambda = Eigen::MatrixXd::Zero(Aee.rows(), 1);
		Eigen::MatrixXd gk;
		gk = H * x + c;
		Eigen::MatrixXd bee = Eigen::MatrixXd::Zero(Aee.rows(), 1);
		qp_lagrange(H, gk, Aee, bee, d, lambda, d.rows(), Aee.rows());

		double d_norm = 0;
		for (int i = 0; i < d.rows(); i++) {
			d_norm += d(i) * d(i);
		}
		// d的模是否=0 是 停算
		if (d_norm < 1e-6) {
			// 终止判断
			double minLambda = 9999;
			int minLambdaIndex = 0;
			for (int i = 0; i < lambda.rows(); i++) {
				if (lambda(i, 0) < minLambda) {
					minLambda = lambda(i, 0);
					minLambdaIndex = i;
				}
			}
			if (minLambda > 0) {
				break;
			}
			// 从有效集中剔除约束
			else {
				int removed_cons = asIndex[minLambdaIndex - ne];
				Sk(removed_cons, 0) = 0;
			}
		}
		// 否 求步长
		else {
			Eigen::MatrixXd ad(Ai.rows(), 1);
			ad = Ai * d;
			Eigen::MatrixXd ax(Ai.rows(), 1);
			ax = Ai * x;
			std::vector<double> alphaList;
			int min_alpha_index = 0;
			double min_alpha = 9999;
			for (int i = 0; i < Sk.rows(); i++) {
				if (Sk(i) == 0 && ad(i, 0) < 0) {
					double alpha_ = (bi(i, 0) - ax(i, 0)) / ad(i, 0);
					if (alpha_ < min_alpha) {
						min_alpha = alpha_;
						min_alpha_index = i;
					}
					alphaList.push_back(alpha_);
				}
			}

			if (min_alpha > 1) {
				x = x + d;
			}
			else {
				x = x + min_alpha * d;
				Sk(min_alpha_index) = 1;	// 将最小的alpha对应的约束加入有效集
			}
		}
		
		iterTimes += 1;
	}
	#pragma endregion
}

?2）光滑牛顿法

对于形如下式(12.42)所示的不等式线性约束二次规划

$min \bigtriangledown f(\bar{x})^{T}d+\frac{1}{2}d^{T}\bigtriangledown^{2}f(\bar{x})d$

$s.t.$

$h(\bar{x})+\bigtriangledown h(\bar{x})^{T}d=0$

? $g(\bar{x})+\bigtriangledown g(\bar{x})^{T}d\geq 0$

式中， $\bar{x}$ 为固定值，与后面 $x_{k}$ 含义相同， $f(x), h(x), g(x)$ 是为了与后文非线性不等式约束的二次规划的子问题对应起来，代入 $\bar{x}$ 后， $f(\bar{x}), h(\bar{x}), g(\bar{x})$ 均为定常数组，注意>=不能反。其中，变量n个，等式约束 $l$ 个，不等式约束m个

$A_{k}^{E}=\bigtriangledown h(\bar{x})^{T}$

? $A_{k}^{I}=\bigtriangledown g(\bar{x})^{T}$

? $(d, \mu, \lambda)\in \mathbb{R}^{n}\times \mathbb{R}^{l} \times \mathbb{R}^{m}$

利用KT条件，式（1）等价于

?注标黄的地方是 $\gamma$ 不是 $\beta$

实现代码：

template<typename Derived>
Eigen::MatrixXd qp_smooth_Newton_method(Eigen::MatrixBase<Derived>& Bk, Eigen::MatrixBase<Derived>& df_xk, Eigen::MatrixBase<Derived>& AkE, Eigen::MatrixBase<Derived>& AkI,
	Eigen::MatrixBase<Derived>& h_xk, Eigen::MatrixBase<Derived>& g_xk,
	int& n, int& l, int& m) {
	// n-未知数个数，对应d  l-等式约束个数 对应mu h(x)  m-不等式约束个数 对应lambda g(x)
 	double epsilon = 0.05;
	double ep0 = 0.05;
	double gamma = 0.05;
	double rho = 0.5;
	double sigma = 0.2;
	Eigen::MatrixXd z = Eigen::MatrixXd::Zero(1 + n + l + m, 1);
	z(0, 0) = epsilon;
	z.block(1, 0, n, 1) = Eigen::MatrixXd::Ones(n, 1);
	int iterTimes = 0;
	while (iterTimes < 150) {
		Eigen::MatrixXd H = form_H(z, n, l, m, Bk, df_xk, AkE, AkI, h_xk, g_xk);
		double norm_H = H.norm();
		// 停算判断
		if (norm_H <= 1e-6) break;
		// 计算 dz
		double min_temp = (1 < norm_H) ? 1 : norm_H;
		double beta = gamma * norm_H * min_temp;
		Eigen::MatrixXd dH = JacobH(z, n, l, m, Bk, AkE, AkI, g_xk);
		Eigen::MatrixXd z_bar = Eigen::MatrixXd::Zero(1 + n + l + m, 1);
		z_bar(0, 0) = epsilon;
		Eigen::MatrixXd dz = dH.inverse() * (beta * z_bar - H);
		// 计算步长alpha
		int i = 0;
		int mk = 0;
		while (mk <= 20) {
			Eigen::MatrixXd z_ = z + pow(rho, i) * dz;
			Eigen::MatrixXd H_rho = form_H(z_, n, l, m, Bk, df_xk, AkE, AkI, h_xk, g_xk);
			double norm_H_rho = H_rho.norm();
			double rho_temp = (1 - sigma * (1 - gamma * ep0) * pow(rho, i)) * norm_H;
			if (norm_H_rho <= rho_temp) {
				mk = i;
				break;
			}
			i += 1;
			if (i == 20) {
				mk = 10;
			}
		}

		double alpha = pow(rho, mk);
		z = z + alpha * dz;
		iterTimes += 1;
	}
	return z.block(1, 0, n, 1);
}

?三、非线性不等式约束二次规划（SQP求解）

对于形如下式的最优化问题

$min f(x)$

$s.t.$

$h(x)=0$

$g(x)\geq 0$

对其线性化

$min \bigtriangledown f(\bar{x})^{T}d+\frac{1}{2}d^{T}\bigtriangledown^{2}f(\bar{x})d$

$s.t.$

$h(\bar{x})+\bigtriangledown h(\bar{x})^{T}d=0$

? $g(\bar{x})+\bigtriangledown g(\bar{x})^{T}d\geq 0$

?可按照有效集发或光滑牛顿法求解子问题额的解，子问题的解作为原问题的步长

$x_{k+1}=x_{k}+d_{k}$

当 $d_{k}\approx 0$ 时停算。

$\bigtriangledown f(x)=\begin{bmatrix} \frac{\partial f}{\partial x_{1}} \\ ... \\ \frac{\partial f}{\partial x_{n}} \end{bmatrix}$

$\bigtriangledown^{2}f(x)=\begin{bmatrix} \frac{\partial^{2} f}{\partial x_{1}\partial x_{1}} & ... & \frac{\partial^{2} f}{\partial x_{1}\partial x_{n}}\\ ... & ... & ...\\ \frac{\partial^{2} f}{\partial x_{n}\partial x_{1}} & & \frac{\partial^{2} f}{\partial x_{n}\partial x_{n}} \end{bmatrix}$

附完整代码

#include <iostream>
#include <Eigen/Dense>
#include <vector>


template<typename Derived>
void qp_lagrange(Eigen::MatrixBase<Derived>& H, Eigen::MatrixBase<Derived>& c, Eigen::MatrixBase<Derived>& A, Eigen::MatrixBase<Derived>& b,
	Eigen::MatrixBase<Derived>& x, Eigen::MatrixBase<Derived>& lambda, const int& dim, const int& m) {
	Eigen::MatrixXd G(dim, dim);
	Eigen::MatrixXd B(m, dim);
	Eigen::MatrixXd C(m, m);

	G = H.inverse() - H.inverse() * A.transpose() * (A * H.inverse() * A.transpose()).inverse() * A * H.inverse();
	B = (A * H.inverse() * A.transpose()).inverse() * A * H.inverse();
	C = -1 * (A * H.inverse() * A.transpose()).inverse();

	x = B.transpose() * b - G * c;
	lambda = B * c - C * b;
}

template<typename Derived>
void qp_activeset(Eigen::MatrixBase<Derived>& H, Eigen::MatrixBase<Derived>& c, Eigen::MatrixBase<Derived>& Ae, Eigen::MatrixBase<Derived>& be,
	Eigen::MatrixBase<Derived>& Ai, Eigen::MatrixBase<Derived>& bi, const int& ne, const int& ni, Eigen::MatrixBase<Derived>& x) {
	double epsilon = 1e-9;
	Eigen::MatrixXd Sk = Eigen::MatrixXd::Zero(ni, 1);
	std::vector<double> asIndex;	//记录第几个不等式约束被选为有效集

	Eigen::MatrixXd bi_left(Ai.rows(), x.cols());
	bi_left = Ai * x;
	Eigen::MatrixXd Aee(1, 1);

	#pragma region set initial active set
	for (int i = 0; i < ni; i++) {
		double bb = bi(i, 0);
		if (bi_left(i, 0) <= bi(i, 0) + epsilon) {
			Sk(i, 0) = 1;
		}
	}
	#pragma endregion

	#pragma region main loop
	int iterTimes = 0;
	while (iterTimes < 150) {
		int activeSetSize = 0;
		for (int i = 0; i < Sk.rows(); i++) {
			if (Sk(i) == 1) {
				activeSetSize += 1;
			}
		}
		if (ne != 0) {
			Aee.resize(ne + activeSetSize, Ae.cols());
			Aee.block(0, 0, Ae.rows(), Ae.cols()) = Ae;
			int aee_index = 0;
			for (int i = 0; i < ni; i++) {
				if (Sk(i, 0) == 1) {
					Aee.block(ne + aee_index, 0, 1, Ae.cols()) = Ai.row(i);
					asIndex.push_back(i);
					aee_index += 1;
				}
			}
		}
		else {
			Aee.resize(activeSetSize, Ai.cols());
			int aee_index = 0;
			for (int i = 0; i < ni; i++) {
				if (Sk(i, 0) == 1) {
					Aee.block(aee_index, 0, 1, Ai.cols()) = Ai.row(i);
					asIndex.push_back(i);
					aee_index += 1;
				}
			}
		}

		// 解子问题
		Eigen::MatrixXd d = Eigen::MatrixXd::Zero(x.rows(), 1);
		Eigen::MatrixXd lambda = Eigen::MatrixXd::Zero(Aee.rows(), 1);
		Eigen::MatrixXd gk;
		gk = H * x + c;
		Eigen::MatrixXd bee = Eigen::MatrixXd::Zero(Aee.rows(), 1);
		qp_lagrange(H, gk, Aee, bee, d, lambda, d.rows(), Aee.rows());

		double d_norm = 0;
		for (int i = 0; i < d.rows(); i++) {
			d_norm += d(i) * d(i);
		}
		// d的模是否=0 是 停算
		if (d_norm < 1e-6) {
			// 终止判断
			double minLambda = 9999;
			int minLambdaIndex = 0;
			for (int i = 0; i < lambda.rows(); i++) {
				if (lambda(i, 0) < minLambda) {
					minLambda = lambda(i, 0);
					minLambdaIndex = i;
				}
			}
			if (minLambda > 0) {
				break;
			}
			// 从有效集中剔除约束
			else {
				int removed_cons = asIndex[minLambdaIndex - ne];
				Sk(removed_cons, 0) = 0;
			}
		}
		// 否 求步长
		else {
			Eigen::MatrixXd ad(Ai.rows(), 1);
			ad = Ai * d;
			Eigen::MatrixXd ax(Ai.rows(), 1);
			ax = Ai * x;
			std::vector<double> alphaList;
			int min_alpha_index = 0;
			double min_alpha = 9999;
			for (int i = 0; i < Sk.rows(); i++) {
				if (Sk(i) == 0 && ad(i, 0) < 0) {
					double alpha_ = (bi(i, 0) - ax(i, 0)) / ad(i, 0);
					if (alpha_ < min_alpha) {
						min_alpha = alpha_;
						min_alpha_index = i;
					}
					alphaList.push_back(alpha_);
				}
			}

			if (min_alpha > 1) {
				x = x + d;
			}
			else {
				x = x + min_alpha * d;
				Sk(min_alpha_index) = 1;	// 将最小的alpha对应的约束加入有效集
			}
		}
		
		iterTimes += 1;
	}
	#pragma endregion
}

#pragma region function
template<typename Derived>
double f(Eigen::MatrixBase<Derived>& x) {
	double x1 = x(0, 0);
	double x2 = x(1, 0);
	double f = 6.0 * x1 / x2 + x2 / (x1 * x1);
	return f;
}
template<typename Derived>
Eigen::MatrixXd h(Eigen::MatrixBase<Derived>& x) {
	double x1 = x(0, 0);
	double x2 = x(1, 0);
	double h_ = x1 * x2 - 2;
	Eigen::MatrixXd h(1, 1);
	h << h_;
	return h;
}
template<typename Derived>
Eigen::MatrixXd g(Eigen::MatrixBase<Derived>& x) {
	// 不等式要大于等于
	double x1 = x(0, 0);
	double x2 = x(1, 0);
	Eigen::MatrixXd g(1, 1);
	double g_ = -1 + x1 + x2;
	g << g_;
	return g;
}
template<typename Derived>
Eigen::MatrixXd gradf(Eigen::MatrixBase<Derived>& x) {
	// 设定f梯度
	Eigen::MatrixXd gf(2, 1);
	double x1 = x(0, 0);
	double x2 = x(1, 0);
	gf << 6.0 / x2 - 2 * x2 / pow(x1, 3),
		-6.0 * x1 / (x2 * x2) + 1.0 / (x1 * x1);

	return gf;
}
template<typename Derived>
Eigen::MatrixXd grad2f(Eigen::MatrixBase<Derived>& x) {
	// 设定f梯度的梯度
	Eigen::MatrixXd g2f(2, 2);
	double x1 = x(0, 0);
	double x2 = x(1, 0);
	g2f << 6.0 * x2 /pow(x1, 4), -6.0 / (x2 * x2) - 2.0 / pow(x1, 3),
		-6.0 / (x2 * x2) - 2.0 / pow(x1, 3), 12.0 * x1 / pow(x2, 3);

	return g2f;
}
template<typename Derived>
Eigen::MatrixXd gradh(Eigen::MatrixBase<Derived>& x) {
	// 设定h梯度
	Eigen::MatrixXd gh(2, 1);
	double x1 = x(0, 0);
	double x2 = x(1, 0);
	gh << x2,
		x1;
	return gh;
}
template<typename Derived>
Eigen::MatrixXd grad2h(Eigen::MatrixBase<Derived>& x) {
	// 设定h梯度的梯度
	Eigen::MatrixXd g2h(2, 2);
	double x1 = x(0, 0);
	double x2 = x(1, 0);
	g2h << 0, 1,
		1, 0;
	return g2h;
}
template<typename Derived>
Eigen::MatrixXd gradg(Eigen::MatrixBase<Derived>& x) {
	// 设定g梯度
	Eigen::MatrixXd gg(2, 1);
	double x1 = x(0, 0);
	double x2 = x(1, 0);
	gg << 1,
		1;
	return gg;
}
template<typename Derived>
Eigen::MatrixXd grad2g(Eigen::MatrixBase<Derived>& x) {
	// 设定g梯度的梯度
	Eigen::MatrixXd g2g(2, 2);
	double x1 = x(0, 0);
	double x2 = x(1, 0);
	g2g << 0, 0,
		0, 0;
	return g2g;
}
#pragma endregion

void sqp_subp_activeSet() {
	// 有效集解子问题 解SQP
	int dim = 2;	// 未知数个数
	int ne = 1;	// 等式约束的个数
	int ni = 1; // 不等式约束的个数
	// 设定4个梯度矩阵
	Eigen::MatrixXd gf(dim, 1);
	Eigen::MatrixXd g2f(dim, dim);
	Eigen::MatrixXd gh(dim, 1);
	Eigen::MatrixXd g2h(dim, dim);
	Eigen::MatrixXd gg(dim, 1);
	Eigen::MatrixXd g2g(dim, dim);
	//给定初始解
	Eigen::MatrixXd x(dim, 1);
	x << 2, 1;
	// 子问题的矩阵
	Eigen::MatrixXd H(dim, dim);
	Eigen::MatrixXd c(dim, 1);
	Eigen::MatrixXd Ae(ne, dim);
	Eigen::MatrixXd be(ne, 1);
	// 设定不等式的时候注意大于号
	Eigen::MatrixXd Ai(ni, dim);
	Eigen::MatrixXd bi(ni, 1);
	// 子问题的解
	Eigen::MatrixXd d(dim, 1);
	int iterTimes = 0;
	while (iterTimes < 100) {
		c = gradf(x);
		H = grad2f(x);
		Ae = gradh(x).transpose();
		Ai = gradg(x).transpose();
		be = -1 * h(x);
		bi = -1 * g(x);
		d << 0, 0;
		qp_activeset(H, c, Ae, be, Ai, bi, ne, ni, d);
		x = x + d;
	}
}

#pragma region Smooth Newton Method
double phi(double& epsilon, double& a, double& b) {
	double p = a + b - sqrt(a * a + b * b + 2 * epsilon * epsilon);
	return p;
}

template<typename Derived>
Eigen::MatrixXd form_H(Eigen::MatrixBase<Derived>& z, int& n, int& l, int& m,
	Eigen::MatrixBase<Derived>& Bk, Eigen::MatrixBase<Derived>& df_xk, Eigen::MatrixBase<Derived>& AkE, Eigen::MatrixBase<Derived>& AkI, Eigen::MatrixBase<Derived>& h_xk, Eigen::MatrixBase<Derived>& g_xk) {
	double epsilon = z(0, 0);
	Eigen::MatrixXd d = z.block(1, 0, n, 1);
	Eigen::MatrixXd mu = z.block(1 + n, 0, l, 1);
	Eigen::MatrixXd lambda = z.block(1 + n + l, 0, m, 1);
	Eigen::MatrixXd H1(d.rows(), 1);
	H1 = Bk * d - AkE.transpose() * mu - AkI.transpose() * lambda + df_xk;
	Eigen::MatrixXd H2(d.rows(), 1);
	H2 = h_xk + AkE * d;
	Eigen::MatrixXd Phi(lambda.rows(), 1);
	Eigen::MatrixXd b = g_xk + AkI * d;
	for (int i = 0; i < lambda.rows(); i++) {
		double p = phi(epsilon, lambda(i, 0), b(i, 0));
		Phi(i, 0) = p;
	}
	Eigen::MatrixXd H(1 + H1.rows() + H2.rows() + Phi.rows(), 1);
	H(0, 0) = epsilon;
	H.block(1, 0, H1.rows(), 1) = H1;
	H.block(1 + H1.rows(), 0, H2.rows(), 1) = H2;
	H.block(1 + H1.rows() + H2.rows(), 0, Phi.rows(), 1) = Phi;
	return H;
}

template<typename Derived>
Eigen::MatrixXd JacobH(Eigen::MatrixBase<Derived>& z, int& n, int& l, int& m, Eigen::MatrixBase<Derived>& Bk, Eigen::MatrixBase<Derived>& AkE, Eigen::MatrixBase<Derived>& AkI, Eigen::MatrixBase<Derived>& g_xk) {
	Eigen::MatrixXd d = z.block(1, 0, n, 1);
	Eigen::MatrixXd mu = z.block(1 + n, 0, l, 1);
	Eigen::MatrixXd lambda = z.block(1 + n + l, 0, m, 1);
	double epsilon = z(0, 0);
	Eigen::MatrixXd temp(lambda.rows(), 1);
	temp = g_xk + AkI * d;
	Eigen::MatrixXd D1 = Eigen::MatrixXd::Zero(lambda.rows(), lambda.rows());
	for (int i = 0; i < lambda.rows(); i++) {
		D1(i, i) = 1 - lambda(i, 0) / sqrt(lambda(i, 0) * lambda(i, 0) + temp(i, 0) * temp(i, 0) + 2 * epsilon * epsilon);
	}

	Eigen::MatrixXd D2 = Eigen::MatrixXd::Zero(lambda.rows(), lambda.rows());
	for (int i = 0; i < lambda.rows(); i++) {
		D2(i, i) = 1 - temp(i, 0) / sqrt(lambda(i, 0) * lambda(i, 0) + temp(i, 0) * temp(i, 0) + 2 * epsilon * epsilon);
	}

	Eigen::MatrixXd nu(lambda.rows(), 1);
	for (int i = 0; i < lambda.rows(); i++) {
		nu(i, 0) = -2 * epsilon / sqrt(lambda(i, 0) * lambda(i, 0) + temp(i, 0) * temp(i, 0) + 2 * epsilon * epsilon);
	}

	Eigen::MatrixXd dH = Eigen::MatrixXd::Zero(1 + Bk.rows() + AkE.rows() + nu.rows(), 1 + Bk.cols() + AkE.rows() + AkI.rows());
	dH.block(0, 0, 1, 1) = Eigen::MatrixXd::Ones(1, 1);
	dH.block(1, 1, Bk.rows(), Bk.cols()) = Bk;
	dH.block(1, 1 + Bk.cols(), AkE.cols(), AkE.rows()) = -1 * AkE.transpose();
	dH.block(1, 1 + Bk.cols() + AkE.rows(), AkI.cols(), AkI.rows()) = -1 * AkI.transpose();
	dH.block(1 + Bk.rows(), 1, AkE.rows(), AkE.cols()) = AkE;
	dH.block(1 + Bk.rows() + AkE.rows(), 0, nu.rows(), nu.cols()) = nu;
	Eigen::MatrixXd D2AkI = D2 * AkI;
	dH.block(1 + Bk.rows() + AkE.rows(), nu.cols(), D2AkI.rows(), D2AkI.cols()) = D2AkI;
	dH.block(1 + Bk.rows() + AkE.rows(), nu.cols() + D2AkI.cols() + AkE.rows(), D1.rows(), D1.cols()) = D1;

	return dH;
}

template<typename Derived>
Eigen::MatrixXd qp_smooth_Newton_method(Eigen::MatrixBase<Derived>& Bk, Eigen::MatrixBase<Derived>& df_xk, Eigen::MatrixBase<Derived>& AkE, Eigen::MatrixBase<Derived>& AkI,
	Eigen::MatrixBase<Derived>& h_xk, Eigen::MatrixBase<Derived>& g_xk,
	int& n, int& l, int& m) {
	// n-未知数个数，对应d  l-等式约束个数 对应mu h(x)  m-不等式约束个数 对应lambda g(x)
 	double epsilon = 0.05;
	double ep0 = 0.05;
	double gamma = 0.05;
	double rho = 0.5;
	double sigma = 0.2;
	Eigen::MatrixXd z = Eigen::MatrixXd::Zero(1 + n + l + m, 1);
	z(0, 0) = epsilon;
	z.block(1, 0, n, 1) = Eigen::MatrixXd::Ones(n, 1);
	int iterTimes = 0;
	while (iterTimes < 150) {
		Eigen::MatrixXd H = form_H(z, n, l, m, Bk, df_xk, AkE, AkI, h_xk, g_xk);
		double norm_H = H.norm();
		// 停算判断
		if (norm_H <= 1e-6) break;
		// 计算 dz
		double min_temp = (1 < norm_H) ? 1 : norm_H;
		double beta = gamma * norm_H * min_temp;
		Eigen::MatrixXd dH = JacobH(z, n, l, m, Bk, AkE, AkI, g_xk);
		Eigen::MatrixXd z_bar = Eigen::MatrixXd::Zero(1 + n + l + m, 1);
		z_bar(0, 0) = epsilon;
		Eigen::MatrixXd dz = dH.inverse() * (beta * z_bar - H);
		// 计算步长alpha
		int i = 0;
		int mk = 0;
		while (mk <= 20) {
			Eigen::MatrixXd z_ = z + pow(rho, i) * dz;
			Eigen::MatrixXd H_rho = form_H(z_, n, l, m, Bk, df_xk, AkE, AkI, h_xk, g_xk);
			double norm_H_rho = H_rho.norm();
			double rho_temp = (1 - sigma * (1 - gamma * ep0) * pow(rho, i)) * norm_H;
			if (norm_H_rho <= rho_temp) {
				mk = i;
				break;
			}
			i += 1;
			if (i == 20) {
				mk = 10;
			}
		}

		double alpha = pow(rho, mk);
		z = z + alpha * dz;
		iterTimes += 1;
	}
	return z.block(1, 0, n, 1);
}

#pragma endregion

void sqp_subp_smoothNewtionMethod() {
	// 有效集解子问题 解SQP
	int dim = 2;	// 未知数个数
	int ne = 1;	// 等式约束的个数
	int ni = 1; // 不等式约束的个数
	// 设定4个梯度矩阵
	Eigen::MatrixXd gf(dim, 1);
	Eigen::MatrixXd g2f(dim, dim);
	Eigen::MatrixXd gh(dim, 1);
	Eigen::MatrixXd g2h(dim, dim);
	Eigen::MatrixXd gg(dim, 1);
	Eigen::MatrixXd g2g(dim, dim);
	//给定初始解
	Eigen::MatrixXd x(dim, 1);
	x << 2, 1;
	// 子问题的矩阵
	Eigen::MatrixXd H(dim, dim);
	Eigen::MatrixXd c(dim, 1);
	Eigen::MatrixXd Ae(ne, dim);
	Eigen::MatrixXd be(ne, 1);
	// 设定不等式的时候注意大于号
	Eigen::MatrixXd Ai(ni, dim);
	Eigen::MatrixXd bi(ni, 1);
	int iterTimes = 0;
	while (iterTimes < 100) {
		c = gradf(x);
		H = grad2f(x);
		Ae = gradh(x).transpose();
		Ai = gradg(x).transpose();
		Eigen::MatrixXd hx = h(x);
		Eigen::MatrixXd gx = g(x);
		Eigen::MatrixXd d = qp_smooth_Newton_method(H, c, Ae, Ai, hx, gx, dim, ne, ni);
		if (d.norm() < 1e-6) break;
		x = x + d;
	}
}

int main()
{
	//int n = 2;
	//int l = 0;
	//int m = 5;
	//Eigen::MatrixXd dfk(n, 1);
	//dfk << -6, -2;
	//Eigen::MatrixXd Bk(n, n);
	//Bk << 1, -1,
	//	-1, 2;
	//Eigen::MatrixXd Ae(l, n);
	//Eigen::MatrixXd hk(l, 1);
	//Eigen::MatrixXd Ai(m, n);
	//Ai << -2, -1,
	//	1, -1,
	//	-1, -2,
	//	1, 0,
	//	0, 1;
	//Eigen::MatrixXd gk(m, 1);
	//gk << 3, 1, 2, 0, 0;

	//Eigen::MatrixXd d = qp_smooth_Newton_method(Bk, dfk, Ae, Ai, hk, gk, n, l, m);

	sqp_subp_smoothNewtionMethod();

	system("pause");
}