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.
