Gauss-Newton optimization can be used for dynamic systems too! Python Example

Sometimes we need to find parameters of some model that we are working on. You might think that this task (which is called “model parameter estimation” by the way) can be solved only with some special tools like Kalman Filter in the estimation process, but as a matter of fact, this stochastic process is sometimes tackled with…optimization tools. Today I will explain how to use the Gauss-Newton method for a simple dynamical system.

Ok, so before we even start let me ask a question: Have you ever tried to find params of an equation? If so, what kind of tool did you use? Let me guess: Least Squares? Most of the parameter estimation problems are solved with the use of regression, either for linear or nonlinear systems. For example, you can simply answer what are the params a and b of an equation y=ax+b, having two vectors y and x. All you need is a line of code:

beta =  numpy.matmul(np.matmul(np.linalg.inv(np.matmul(numpy.transpose(X), X)), numpy.transpose(X)), Y)

realizing this:

where:

But what in case if a system is dynamic and at the same time nonlinear? Well, we cannot use the above, instead, we use one of nonlinear least squared techniques e.g. GN optimization.

1. Math model

Let’s start with an example of a nonlinear dynamic system. Dynamic systems are those which couples variables and their derivatives e.g. velocity and position:

In our case, we’re going to derive an equation describing vehicle velocity, on which two forces are acting i.e. thrust and drag:

As we can see the drag force is friction which depends on one parameter and squared velocity. The second parameter is a vehicle’s mass m.

We need a dynamic system, so we introduce velocity derivative:

and finally, we need to have a discrete form of the above equation, where the left part is the current sample of the velocity (sth sample):

In the model above, we need to provide force and some parameters i.e mass friction, and sampling time.

import numpy as np
class Vehicle:
def __init__(self, x_in, dt, m, u, n):
        self.x = x_in
        self.m = m  # total mass
        self.u = u  # drag coefficient 
        self.n = n  # size of input vector
        self.dt = dt  # sampling time
        self.v = np.zeros(n)  # velocity
def simulate(self):
        self.v[0] = 0
        for i in range(0, self.n):
            if i == 0:
                self.v[i] = self.dt * self.x[i] / self.m
            else:
                self.v[i] = self.v[i-1] + (self.x[i]/self.m - (self.u/self.m)*self.v[i-1]*self.v[i-1])*self.dt
        print(self.v)
        return self.v

We can simply simulate velocity with the use of the class above and the following code:

x = np.array([1000, 1200, 800, 500, 1800, 0, 1900, 600]) #force [N]
vehicle = Vehicle(x, 20, 1500, 0.1, 8) # dt:=20[s], mass:=1500[kg] #drag coef.= 0.1, vector size:=8
v = vehicle.simulate()

3. The optimizer

Now, we will proceed to optimization. As the post’s title says the optimizer will be based on the Gauss-Newton method.

The whole GN algorithm is simple, but since it was designed to deal with nonlinearities there is a need to calculate the Jacobian matrix. Here is the GN algorithm:

You need to provide the algorithm with some high-value number m_max and precision number e. Note that the Jacobian matrix elements at ith-row and jth-column are calculated before the actual procedure. Let’s find them!

The dynamic system described above has to be given in terms of vectors x, y, and Beta. First, we rewrite our model so it uses only predefined vectors and variables (note that x and y are scalars, not vectors):

Then we need to find the residue r:

Note that y with subscript true is the actual velocity measured in the system.

Having the residue, we derive analytically the Jacobian matrix row. That’s true — row! we need to find analytically one row. This is because all rows have the same analytical form. We only substitute consecutive vector elements into this form later in the code.

Ok, that’s it! Now a code of our optimizer:

import numpy as np
class GaussNetwonVehicle:
def __init__(self, x_in, y_in, dt):
state_size = 2  # how many parameters we search for
self.beta = [10, 10]  # m and u, zeros here may lead to serious problems np.zeros(state_size)
self.dt = dt  # sampling time in model
        self.r_s = y_in.shape[0]  # count how many residuals we have
        self.x = x_in  # opt input; time and force
        self.y = y_in   # target output; velocity
        self.J = np.zeros([self.r_s, state_size])  # Jacobian matrix
        self.r = np.zeros(self.r_s)  # residual vector
        #self.Jr = np.zeros(state_size)
def set_jacobian(self):
        # here define the Jacobian matrix row - only the first, the rest will be calculated based on the first
        for i in range(0, self.r_s):
            if i == 0:
                self.J[i, 0] = (self.x[i]/(self.beta[0]*self.beta[0]))*self.dt - \
                           (self.beta[1]/(self.beta[0]*self.beta[0]))*self.dt*0
                self.J[i, 1] = (1.0 / self.beta[0]) * self.dt * 0
            else:
                self.J[i, 0] = (self.x[i] / (self.beta[0] * self.beta[0])) * self.dt - \
                               (self.beta[1] / (self.beta[0] * self.beta[0])) * self.dt * self.y[i - 1] * self.y[i - 1]
                self.J[i, 1] = (1.0/self.beta[0])*self.dt*self.y[i-1]*self.y[i-1]
    def calc_res(self):
        err = 0
        for i in range(0, self.r_s):
            if i == 0:
                self.r[i] = self.y[i] - self.dt*self.x[i]/self.beta[0] + (self.beta[1]/self.beta[0])*self.dt*0
            else:
                self.r[i] = self.y[i] - self.y[i - 1] - self.dt * self.x[i] / self.beta[0] + \
                            (self.beta[1] / self.beta[0]) * self.dt*self.y[i - 1] * self.y[i - 1]
            err = err + self.r[i]*self.r[i]
        return err
def opt_step(self):
        self.set_jacobian()
        err = self.calc_res()
        Jt = np.transpose(self.J)
        self.beta = self.beta - np.matmul(np.matmul(np.linalg.inv(np.matmul(Jt, self.J)), Jt), self.r)
        print(self.beta)
        return err
def opt(self, e):
err = 100
        errp = 10
        while np.abs(err-errp) > e:
            errp = err
            err = self.opt_step()

And finally, we can execute the whole process, with input data:

from src.vehicle import Vehicle
from src.gn_vehicle import GaussNetwonVehicle
import numpy as np
import matplotlib.pyplot as plt
xv = np.array([1000, 1200, 800, 500, 1800, 0, 1900, 600]) #forces
vehicle = Vehicle(xv, 20, 1500, 0.1, 8)
mu, sigma = 0, 0.5 # mean and standard deviation
s = np.random.normal(mu, sigma, 8)
yv = vehicle.simulate() + s
gnv = GaussNetwonVehicle(xv, yv, 20)
gnv.opt(0.0001)
vehicle = Vehicle(xv, 20, gnv.beta[0], gnv.beta[1], 8)
yvs = vehicle.simulate()
t = list(range(0, 160, 20))
plt.plot(t, yv, 'g', label='original')
plt.plot(t, yvs, 'r', label='simulated')
plt.ylabel('[m/s]')
plt.xlabel('[s]')
plt.title('vehicle speed')
plt.legend()
plt.show()

We get the result:

[19.9357714   9.93634914]
[39.61627587  9.81027127]
[78.22451638  9.56293793]
[152.51883648   9.08699128]
[290.09681853   8.20563483]
[526.1413096    6.69347908]
[874.48199955   4.46192783]
[1257.79582773    2.00633056]
[1.49946284e+03 4.58155629e-01]
[1.55481699e+03 1.03544130e-01]
[1.55693588e+03 8.99700370e-02]

In the results, the first param is a vehicle’s mass, the second is a friction coefficient. The results are close to the original values.

So in the code above, we create a vehicle and we simulate it to get the velocity. Later, we add noise to the velocity and pass this data through the GN optimizer. In the end, we plot the result. You can play with the vehicle’s params and the noise to observe how it influences the process.

Summary

We did quite a lot! We managed to create a dynamic system consisting of a vehicle velocity-force model. We created a GN optimizer, able to solve our problem and we found model params that we had searched for.

You can easily change the classes given above to address various problems. I hope you will find it useful.

Project: https://github.com/jaroslav87/gauss_netwon