Using GEKKO for Moving Horizon Estimation online 2

This is the following question after appyling comments from: Using GEKKO for Moving Horizon Estimation online

I have studied example from estimation iterative example on the Dynamic Optimization course website and revised my code as follows:

from gekko import GEKKO
import numpy as np
import matplotlib.pyplot as plt
import matplotlib; matplotlib.use('TkAgg')

class Observer():

    def __init__(self, window_size, r_init, alpha_init):
        self.m = GEKKO(remote=False)
        self.dt = 0.05
        self.m.time = [i*self.dt for i in range(window_size)]
        #Parameters
        self.m.u = self.m.MV()
        #Variables
        self.m.r = self.m.CV(lb=0) # value=r_init) #ub=20 can be over 20
        self.m.alpha = self.m.CV() # value=alpha_init) #ub lb for angle?
        #Equations
        self.m.Equation(self.m.r.dt()== -self.m.cos(self.m.alpha))
        self.m.Equation(self.m.alpha.dt()== self.m.sin(self.m.alpha)/self.m.r - self.m.u)  # differential equation
        #Options
        self.m.options.MV_STEP_HOR = 2
        self.m.options.IMODE = 5   # dynamic estimation
        self.m.options.EV_TYPE = 2 #Default 1: absolute error form 2: squared error form
        self.m.options.DIAGLEVEL = 0 #diagnostic level
        self.m.options.NODES = 5 #nodes   # collocation nodes default:2
        self.m.options.SOLVER = 3 #solver_num

        # STATUS = 0, optimizer doesn't adjust value
        # STATUS = 1, optimizer can adjust
        self.m.u.STATUS = 0
        self.m.r.STATUS = 1
        self.m.alpha.STATUS = 1
        # FSTATUS = 0, no measurement
        # FSTATUS = 1, measurement used to update model
        self.m.u.FSTATUS = 1 #default
        self.m.r.FSTATUS = 1
        self.m.alpha.FSTATUS = 1

        self.m.r.TR_INIT = 0
        self.m.alpha.TR_INIT = 0

        self.count = 0

    def MHE(self, observed_state, u_data):
        self.count =+ 1
        self.m.u.MEAS = u_data
        self.m.r.MEAS = observed_state[0]
        self.m.alpha.MEAS = observed_state[1]
        self.m.solve(disp=False)
        return self.m.r.MODEL, self.m.alpha.MODEL

if __name__=="__main__":
    FILE_PATH00 = '/home/shane16/Project/model_guard/uav_paper/adversarial/SA_PPO/src/DATA/4end_estimation_results_r.csv'
    FILE_PATH01 = '/home/shane16/Project/model_guard/uav_paper/adversarial/SA_PPO/src/DATA/4end_estimation_results_alpha.csv'
    FILE_PATH02 = '/home/shane16/Project/model_guard/uav_paper/adversarial/SA_PPO/src/DATA/4end_action_buffer_eps0.0_sig0.0.csv'
    cycles = 55
    
    x = np.arange(cycles) # 1...300
    matrix00 = np.loadtxt(FILE_PATH00, delimiter=',')
    matrix01 = np.loadtxt(FILE_PATH01, delimiter=',')
    matrix02 = np.loadtxt(FILE_PATH02, delimiter=',')

    vanilla_action_sigma_0 = matrix02

    vanilla_estimation_matrix_r = np.zeros(cycles)
    vanilla_estimation_matrix_alpha = np.zeros(cycles)

    # sigma = 0.0
    # vanilla model true/observed states
    r_vanilla_sigma_0_true = matrix00[0, 3:] # from step 1
    r_vanilla_sigma_0_observed = matrix00[1, 3:] # from step1
    alpha_vanilla_sigma_0_true = matrix01[0, 3:]
    alpha_vanilla_sigma_0_observed = matrix01[1, 3:]

    # initialize estimator
    sigma = 0.0 #1.0
    solver_num = 3
    nodes = 5
    # for window_size in [5, 10, 20, 30, 40, 50]:
    window_size = 5
    observer = Observer(window_size, r_vanilla_sigma_0_observed[0], alpha_vanilla_sigma_0_observed[0])
    for i in range(cycles):
        if i % 100 == 0:
            print('cylcle: {}'.format(i))
        vanilla_observed_states = np.hstack((r_vanilla_sigma_0_observed[i], alpha_vanilla_sigma_0_observed[i])) # from current observed state
        r_hat, alpha_hat = observer.MHE(vanilla_observed_states, vanilla_action_sigma_0[i]) # and current action -> estimate current state
        vanilla_estimation_matrix_r[i] = r_hat
        vanilla_estimation_matrix_alpha[i] = alpha_hat

    #plot vanilla
    plt.figure()
    plt.subplot(3,1,1)
    plt.title('Vanilla model_sig{}'.format(sigma))
    plt.plot(x, vanilla_action_sigma_0[:cycles],'b:',label='action (w)')
    plt.legend()
    plt.subplot(3,1,2)
    plt.ylabel('r')
    plt.plot(x, r_vanilla_sigma_0_true[:cycles], 'k-', label='true_r')
    plt.plot(x, r_vanilla_sigma_0_observed[:cycles], 'gx', label='observed_r')
    plt.plot(x, vanilla_estimation_matrix_r, 'r--', label='time window: 10')
    # plt.legend()
    plt.subplot(3,1,3)
    plt.xlabel('time steps')
    plt.ylabel('alpha')
    plt.plot(x, alpha_vanilla_sigma_0_true[:cycles], 'k-', label='true_alpha')
    plt.plot(x, alpha_vanilla_sigma_0_observed[:cycles], 'gx', label='observed_alpha')
    plt.plot(x, vanilla_estimation_matrix_alpha, 'r--', label='time window: {}'.format(window_size))
    plt.legend()
    plt.savefig('plot/revision/4estimated_STATES_vanilla_sig{}_window{}_cycles{}_solver{}_nodes{}.png'.format(sigma, window_size,cycles, solver_num, nodes))
    plt.show()

csv files: https://drive.google.com/drive/folders/1jW_6zBCdbJHB7yU3HmCIhamEyOT1LJqD?usp=sharing

The code works when initialized with values specified at line 15,16 (m.r, m.alpha).

However, if I try with no initial value,(as same condition in example), solution is not found.

terminal output:

cylcle: 0 Traceback (most recent call last): File "4observer_mhe.py", line 86, in r_hat, alpha_hat = observer.MHE(vanilla_observed_states, vanilla_action_sigma_0[i]) # and current action -> estimate current state File "4observer_mhe.py", line 49, in MHE self.m.solve(disp=False) File "/home/shane16/Project/model_guard/LipSDP/lipenv/lib/python3.7/site-packages/gekko/gekko.py", line 2140, in solve raise Exception(apm_error) Exception: @error: Solution Not Found

What could be the solution to this problem? I have tried below strategies, but couldn't find the solution.

1. Reduce the number of decision variables by using m.FV() or m.MV() with m.options.MV_STEP_HOR=2+ to reduce the degrees of freedom for the solver for the unknown parameters.
2. Try other solvers with m.options.SOLVER=1 or m.options.SOLVER=2.

I expect to see estimation results that follow the true state well. But I guess I'm doing something wrong. Could anyone help me please?

Thank you.

Solvers sometimes need good initial guess values or constraints (lower and upper bounds) on the degrees of freedom (MV or FV) to find the optimal solution.

One of the equations may be the source of the problem:

self.m.alpha.dt() == self.m.sin(self.m.alpha)/self.m.r - self.m.u

The initial value of r is zero (default) because no initial value is provided when it is declared as self.m.r = self.m.CV(lb=0). A comment suggests that it was formerly initialized with value r_init. The zero value creates a divide-by-zero for that equation. Try rearranging the equation into an equivalent form that avoids the potential for divide-by-zero either with the initial guess or when the solver is iterating.

self.m.r*self.m.alpha.dt() == self.m.sin(self.m.alpha) - self.m.r*self.m.u

There may be other things that are also causing the model to not converge. When the solution does not converge then the infeasibilities.txt file can be a source to troubleshoot the specific equations that are having trouble. Here are instructions to retrieve the infeasibilities.txt file: How to retrieve the 'infeasibilities.txt' from the gekko