Using GEKKO for Moving Horizon Estimation online

Question

I used GEKKO to estimate the states online given perturbed observation states.

But I have two problems:

1. solution is not found for setting window_size* > 40 ex) raise Exception(apm_error) Exception: @error: Solution Not Found
2. using window_size*<40 the solver works, but filtering is not working properly for noisy observed data. ex) red dotted line: estimated state, black line: true state, green x: observed state

*'window_size' is the variable setting the length of the time window of MHE. For detailed use, refer to my code below.

Studying the MHE code example, I found out that what I am trying to do is a little different from the example: Example estimates the states given all the data at once. I want to estimate every time step, receiving the observed data online and estimating the true state. That is, I expect to attain estimated states which aligns with the true state(black line in figure above)

So I made a 'make_buffer' function for updating the observed, control data with a size of time_window. I used this buffer to estimate new states given observed states. I used the last element of the estimated states to plot the results. However, I am not sure if I am implementing the MHE in the right way. Or do I need to use another method for what I want to do?

I tried increasing WMODEL expecting that the estimated results will smoothen, but changing this parameter doesn't seem to make any change :(

My code:

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

class Observer():

    def __init__(self, true_sigma, window_size):
        self.window_size = window_size
        self.r_sigma_list = np.zeros(window_size)
        self.alpha_sigma_list = np.zeros(window_size)
        self.dt = 0.05
        self.t_data = [i*self.dt for i in range(window_size)]
        self.count = 0
        self.true_sigma = true_sigma
        self.estimation_error = 0
        self.tolerance = 0

    def make_buffer(self, observed_state, u_data): # start with 1 data and stack to window_size
        if self.count == 0:
            self.observed_states = observed_state
            self.u_datas = u_data
        elif self.count > 0 and self.count < self.window_size:
            self.observed_states = np.vstack([self.observed_states, observed_state])
            self.u_datas = np.append(self.u_datas, u_data)
        else:
            self.observed_states = np.roll(self.observed_states, -1, axis=0)
            self.observed_states[self.window_size -1] = observed_state
            self.u_datas = np.roll(self.u_datas, -1)
            self.u_datas[self.window_size -1] = u_data
        self.count += 1

    def MHE(self, observed_state, u_data):
        self.make_buffer(observed_state, u_data)
        print('count: {}'.format(self.count))
        if self.count < 2:
            self.r_sigma = 0
            self.alpha_sigma = 0
            # return observed_state[0,0], observed_state[0,1], self.r_sigma, self.alpha_sigma # for online running: states are tensor
            return observed_state[0], observed_state[1], self.r_sigma, self.alpha_sigma # for offline running
        r_data = self.observed_states[:,0]
        # print('r_data: {}'.format(r_data))
        alpha_data = self.observed_states[:,1]
        # print('alpha_data: {}'.format(alpha_data))
        self.m = GEKKO(remote=False)
        self.m.time = self.t_data[:self.count]
        # print('self.m.time: ',self.m.time)
        self.m.r = self.m.CV(value=r_data, lb=0) #ub=20 can be over 20
        self.m.r.STATUS = 0
        self.m.r.FSTATUS = 1 # fit to measurement
        self.m.r.WMODEL = 20.0
        self.m.r.WMEAS = 10.0
        self.m.alpha = self.m.CV(value=alpha_data)
        self.m.alpha.STATUS = 0
        self.m.alpha.FSTATUS = 1
        self.m.alpha.WMODEL = 20.0
        self.m.alpha.WMEAS = 10.0
        # print('u_datas: ', self.u_datas)
        # u = self.m.MV(value=self.u_datas, lb=-2.3, ub=2.3) #MV = FV: 0.12 error ~ SV with no status setting = CV: 0.16 error
        self.m.u = self.m.MV(value=self.u_datas)
        self.m.u.STATUS = 0  # optimizer doesn't adjust value
        self.m.u.FSTATUS = 1 #default
        self.m.Equations([self.m.r.dt()== -self.m.cos(self.m.alpha), self.m.alpha.dt()== self.m.sin(self.m.alpha)/self.m.r - self.m.u])  # differential equation
        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.NODES = 5   # collocation nodes
        # r.MEAS_GAP = 0.1 # MEAS_GAP = dead-band for measurement / model mismatch #used for EV_TYPE = 1
        # alpha.MEAS_GAP = 1
        # try:
        self.m.solve(disp=False)   # display solver output
            # self.r_sigma = np.sqrt(np.sum(np.square(r_data - self.m.r.value))/(self.window_size -1)) #MSE bessel's correction needed: since r.value is estimate?
            # print('r_data - r.value = {} - {} = {}'.format(r_data, self.m.r.value, r_data-self.m.r.value))
            # self.alpha_sigma = np.sqrt(np.sum(np.square(alpha_data - self.m.alpha.value))/(self.window_size -1))
        # except:
        #     print('solution not found')
        return self.m.r.value[-1], self.m.alpha.value[-1], self.r_sigma, self.alpha_sigma

if __name__=="__main__":
    FILE_PATH00 = '/home/shane16/Project/model_guard/uav_paper/adversarial/SA_PPO/src/DATA/estimation_results_r.csv'
    FILE_PATH01 = '/home/shane16/Project/model_guard/uav_paper/adversarial/SA_PPO/src/DATA/estimation_results_alpha.csv'
    FILE_PATH02 = '/home/shane16/Project/model_guard/uav_paper/adversarial/SA_PPO/src/DATA/action_buffer_eps0.0_sig1.0.csv'
    x = np.arange(1,301) # 1...300
    matrix00 = np.loadtxt(FILE_PATH00, delimiter=',')
    matrix01 = np.loadtxt(FILE_PATH01, delimiter=',')
    matrix02 = np.loadtxt(FILE_PATH02, delimiter=',')
    # vanilla model true/observed states
    r_vanilla_sigma_1_true = matrix00[18, 3:]
    r_vanilla_sigma_1_observed = matrix00[19, 3:]
    alpha_vanilla_sigma_1_true = matrix01[18, 3:]
    alpha_vanilla_sigma_1_observed = matrix01[19, 3:]
    vanilla_estimation_matrix_r = np.zeros(300)
    vanilla_estimation_matrix_alpha = np.zeros(300)
    # u_data
    vanilla_action_sigma_1 = matrix02
    # initialize estimator
    sigma = 1.0
    window_size = 10
    observer = Observer(sigma, window_size)

    for i in range(300):
        vanilla_observed_states = np.hstack((r_vanilla_sigma_1_observed[i], alpha_vanilla_sigma_1_observed[i]))
        # print('vanilla_observed_states: ', vanilla_observed_states)
        r_hat, alpha_hat, estimated_r_sigma, estimated_alpha_sigma = observer.MHE(vanilla_observed_states, vanilla_action_sigma_1[i])
        vanilla_estimation_matrix_r[i] = r_hat
        vanilla_estimation_matrix_alpha[i] = alpha_hat

            # #------------------- save estimated_states as csv ------------------------------------
        # save estimated_r
    plt.figure()
    plt.subplot(3,1,1)
    plt.title('Vanillla model_sig1')
    plt.plot(x, vanilla_action_sigma_1,'b:',label='action (w)')
    plt.legend()
    plt.subplot(3,1,2)
    plt.ylabel('r')
    plt.plot(x, r_vanilla_sigma_1_true, 'k-', label='true_r')
    plt.plot(x, r_vanilla_sigma_1_observed, 'gx', label='observed_r')
    plt.plot(x, vanilla_estimation_matrix_r, 'r--', label='time window: 50')
    # plt.legend()
    plt.subplot(3,1,3)
    plt.xlabel('time steps')
    plt.ylabel('alpha')
    plt.plot(x, alpha_vanilla_sigma_1_true, 'k-', label='true_alpha')
    plt.plot(x, alpha_vanilla_sigma_1_observed, 'gx', label='observed_alpha')
    plt.plot(x, vanilla_estimation_matrix_alpha, 'r--', label='time window: 50')
    # plt.legend()
    plt.savefig('offline_test_estimated_std1_vanilla_window50.png')
    plt.show()

csv files: https://drive.google.com/drive/folders/1mdTkM4kyysYVpXDdRoUE1DMQWEgt-CDQ?usp=sharing

Could you please help me figure this out? Thanks for your help in advance.

Sincerely, from Shane K.

Answer 1

Gekko has an internal database (buffer) to keep track of states, measurements, and other required information for MHE. There is a estimation iterative example on the Dynamic Optimization course website (see the Gekko source code, also available in Matlab / Simulink).

#%%Import packages
import numpy as np
from random import random
from gekko import GEKKO
import matplotlib.pyplot as plt

#%% Process
p = GEKKO()

p.time = [0,.5]

#Parameters
p.u = p.MV()
p.K = p.Param(value=1) #gain
p.tau = p.Param(value=5) #time constant

#variable
p.y = p.SV() #measurement

#Equations
p.Equation(p.tau * p.y.dt() == -p.y + p.K * p.u)

#options
p.options.IMODE = 4

#%% MHE Model
m = GEKKO()

m.time = np.linspace(0,20,41) #0-20 by 0.5 -- discretization must match simulation

#Parameters
m.u = m.MV() #input
m.K = m.FV(value=3, lb=1, ub=3) #gain
m.tau = m.FV(value=4, lb=1, ub=10) #time constant

#Variables
m.y = m.CV() #measurement

#Equations
m.Equation(m.tau * m.y.dt() == -m.y + m.K*m.u)

#Options
m.options.IMODE = 5 #MHE
m.options.EV_TYPE = 1
m.options.DIAGLEVEL = 0

# STATUS = 0, optimizer doesn't adjust value
# STATUS = 1, optimizer can adjust
m.u.STATUS = 0
m.K.STATUS = 1
m.tau.STATUS = 1
m.y.STATUS = 1

# FSTATUS = 0, no measurement
# FSTATUS = 1, measurement used to update model
m.u.FSTATUS = 1
m.K.FSTATUS = 0
m.tau.FSTATUS = 0
m.y.FSTATUS = 1

# DMAX = maximum movement each cycle
m.K.DMAX = 1
m.tau.DMAX = .1

# MEAS_GAP = dead-band for measurement / model mismatch
m.y.MEAS_GAP = 0.25

m.y.TR_INIT = 1

#%% problem configuration
# number of cycles
cycles = 50
# noise level
noise = 0.25

#%% run process, estimator and control for cycles
y_meas = np.empty(cycles)
y_est = np.empty(cycles)
k_est = np.empty(cycles)
tau_est = np.empty(cycles)
u_cont = np.empty(cycles)
u = 2.0

# Create plot
plt.figure(figsize=(10,7))
plt.ion()
plt.show()

for i in range(cycles):
    # change input (u)
    if i==10:
        u = 3.0
    elif i==20:
        u = 4.0
    elif i==30:
        u = 1.0
    elif i==40:
        u = 3.0
    u_cont[i] = u

    ## process simulator
    #load u value
    p.u.MEAS = u_cont[i]
    #simulate
    p.solve()
    #load output with white noise
    y_meas[i] = p.y.MODEL + (random()-0.5)*noise

    ## estimator
    #load input and measured output
    m.u.MEAS = u_cont[i]
    m.y.MEAS = y_meas[i]
    #optimize parameters
    m.solve()
    #store results
    y_est[i] = m.y.MODEL 
    k_est[i] = m.K.NEWVAL 
    tau_est[i] = m.tau.NEWVAL 

    plt.clf()
    plt.subplot(4,1,1)
    plt.plot(y_meas[0:i])
    plt.plot(y_est[0:i])
    plt.legend(('meas','pred'))
    plt.ylabel('y')
    plt.subplot(4,1,2)
    plt.plot(np.ones(i)*p.K.value[0])
    plt.plot(k_est[0:i])
    plt.legend(('actual','pred'))
    plt.ylabel('k')
    plt.subplot(4,1,3)
    plt.plot(np.ones(i)*p.tau.value[0])
    plt.plot(tau_est[0:i])
    plt.legend(('actual','pred'))
    plt.ylabel('tau')
    plt.subplot(4,1,4)
    plt.plot(u_cont[0:i])
    plt.legend('u')
    plt.draw()
    plt.pause(0.05)

There are also examples (see TCLab E and TCLab H) that run real-time with the TCLab micro-controller.

These examples should help to simplify your code so that you don't need to do the work to update the window of states yourself.

Regarding the failure at Windows>40, try the following methods:

• 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.
• Another strategy to reduce the solution time is to let Gekko manage the time shift so that the prior solution is fed back as the initial guess when m.options.TIME_SHIFT=1.
• Try other solvers with m.options.SOLVER=1 or m.options.SOLVER=2.