Using GEKKO for Moving Horizon Estimation online 2 - gekko

Question

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.
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.
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.

Answer 1

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