等式约束的外罚函数

import mpl_toolkits.axisartist as axisartist
import matplotlib.pyplot as plt
from 实用优化算法.helper import*    # 单独博客介绍
from mpl_toolkits.mplot3d import Axes3D

x1,x2,x3 = symbols('x1,x2,x3')

def get_f(seta):
    # return (3/2)*x1**2 + x2**2 + (1/2)*x3**2 - x1*x2 - x2*x3 + x1 + x2 + x3 + seta* pow(x1 + 2*x2 + x3 - 4,2)
    return (x1 - 2)**4 + (x1 - 2*x2)**2 + seta * (x1**2 - x2)**2
def get_len_grad_of_P(x0,seta):
    f  = get_f(seta)
    if len(x0) == 3:
        grad = [[],[],[]]
        grad[0].append(diff(f,x1).subs(x1,x0[0][0]).subs(x2,x0[1][0]).subs(x3,x0[2][0]))
        grad[1].append(diff(f,x2).subs(x1,x0[0][0]).subs(x2,x0[1][0]).subs(x3,x0[2][0]))
        grad[2].append(diff(f,x3).subs(x1,x0[0][0]).subs(x2,x0[1][0]).subs(x3,x0[2][0]))
        return math.sqrt(pow(grad[0][0], 2) + pow(grad[1][0], 2) + pow(grad[2][0], 2))
    elif len(x0) == 2:
        grad = [[], []]
        grad[0].append(diff(f, x1).subs(x1, x0[0][0]).subs(x2, x0[1][0]))
        grad[1].append(diff(f, x2).subs(x1, x0[0][0]).subs(x2, x0[1][0]))
        return math.sqrt(pow(grad[0][0], 2) + pow(grad[1][0], 2))

def get_len_c_x_seta(x0):
    return abs((x1**2 - x2).subs(x1, x0[0][0]).subs(x2, x0[1][0]))
    # return abs((x1 + 2*x2 + x3 - 4).subs(x1, x0[0][0]).subs(x2, x0[1][0]).subs(x3, x0[2][0]))
def do_work(next_x0,seta):
    if len(next_x0) == 3:
        step_x1 = [next_x0[0][0]]
        step_x2 = [next_x0[1][0]]
        step_x3 = [next_x0[2][0]]
    if len(next_x0) == 2:
        step_x1 = [next_x0[0][0]]
        step_x2 = [next_x0[1][0]]
    t = np.identity(2)
    cnt = 1
    while get_len_grad_of_P(next_x0,seta) > 0.1:
        next_x0 = FR_Newton(next_x0, get_f(seta))
        # next_x0 = DFP(next_x0,t,get_f(seta))
        if get_len_c_x_seta(next_x0) <= 0.1:
            if len(next_x0) == 3:
                step_x1.append(next_x0[0][0])
                step_x2.append(next_x0[1][0])
                step_x3.append(next_x0[2][0])
            if len(next_x0) == 2:
                step_x1.append(next_x0[0][0])
                step_x2.append(next_x0[1][0])
            break

        seta = 10 * seta   # 设定seta的增长倍数 c == 10
        print(cnt,next_x0)
        cnt += 1
        if len(next_x0) == 3:
            step_x1.append(next_x0[0][0])
            step_x2.append(next_x0[1][0])
            step_x3.append(next_x0[2][0])
        if len(next_x0) == 2:
            step_x1.append(next_x0[0][0])
            step_x2.append(next_x0[1][0])
    print(next_x0,seta)
    if len(next_x0) == 3:
        return step_x1,step_x2,step_x3
    if len(next_x0) == 2:
        return step_x1, step_x2

if __name__ == '__main__':
    # x0 = [[0],[0],[0]]
    x0 = [[0],[0]]
    print('起始点',x0)
    x0 = np.array(x0)
    step_x1,step_x2 = do_work(x0, 10)

    x_arange = np.linspace(-1, 3.0, 256)
    y_arange = np.linspace(-1, 3.0, 256)
    X, Y = np.meshgrid(x_arange, y_arange)
    Z1 = (X - 2)**4 + (X - 2*Y)**2
    Z2 = X**2 - Y
    # Z3 = X + Y
    plt.xlabel('x')
    plt.ylabel('y')
    plt.contourf(X, Y, Z1, 18, alpha=0.75, cmap='rainbow')
    plt.contourf(X, Y, Z2, 18, alpha=0.75, cmap='rainbow')
    # plt.contourf(X, Y, Z3, 8, alpha=0.75, cmap='rainbow')
    C1 = plt.contour(X, Y, Z1, 88, colors='black')
    C2 = plt.contour(X, Y, Z2, 18, colors='blue')
    # C3 = plt.contour(X, Y, Z3, 8, colors='red')
    # plt.clabel(C1, inline=1, fontsize=10)
    # plt.clabel(C2, inline=1, fontsize=10)
    # plt.clabel(C3, inline=1, fontsize=10)
    plt.plot(step_x1,step_x2,marker = '*',c = 'r')
    plt.show()


    fig = plt.figure()
    ax = Axes3D(fig)
    x_arange = np.arange(-5.0, 5.0)
    y_arange = np.arange(-5.0, 5.0)
    X, Y = np.meshgrid(x_arange, y_arange)
    Z1 = (X - 2)**4 + (X - 2*Y)**2
    Z2 = X**2 - Y
    # Z3 = X + Y
    plt.xlabel('x')
    plt.ylabel('y')
    ax.plot_surface(X, Y, Z1, rstride=1, cstride=1, cmap='BuGn')
    ax.plot_surface(X, Y, Z2, rstride=1, cstride=1, cmap='Blues')
    # ax.plot_surface(X, Y, Z3, rstride=1, cstride=1, cmap='rainbow')
    plt.show()



#  第一个  解  0.9456  0.8941

#  第二个  解  0.3889 1.2222 1.1667

# 课外去做 外罚函数--BFGS 拟牛顿矫正