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:

solution is not found for setting window_size* > 40 ex) raise Exception(apm_error) Exception: @error: Solution Not Found
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.

user_2366941 · Answer

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.

Using GEKKO for Moving Horizon Estimation online

1 Answers