Hamiltonian Servo: Control and Estimation of a Large Team of Autonomous Robotic Vehicles

Vladimir Ivancevic¹, Peyam Pourbeik²

¹Joint and Operations Analysis Division, Defence Science & Technology Group, Canberra, Australia.
²CEWD, Assured Communications, Survivable Networks, Defence Science & Technology Group, Adelaide, Australia.

DOI: 10.4236/ica.2017.84014                

Abstract

This paper proposes a novel Hamiltonian servo system, a combined modeling framework for control and estimation of a large team/fleet of autonomous robotic vehicles. The Hamiltonian servo framework represents high-dimensional, nonlinear and non-Gaussian generalization of the classical Kalman servo system. After defining the Kalman servo as a motivation, we define the affine Hamiltonian neural network for adaptive nonlinear control of a team of UGVs in continuous time. We then define a high-dimensional Bayesian particle filter for estimation of a team of UGVs in discrete time. Finally, we formulate a hybrid Hamiltonian servo system by combining the continuous-time control and the discrete-time estimation into a coherent framework that works like a predictor-corrector system.

Keywords

Team of UGVs, Kalman Servo, Hamiltonian Control, Bayesian Estimation

1. Introduction

Today’s military forces face an increasingly complex operating environment, whether dealing with humanitarian operations or in the theater of war. In many situations one has to deal with a wide variety of issues from logistics to lack of communications infrastructure [1] . In many situations one may be even denied satellite access and hence unable to utilize services such as GPS for Geo-location and the ability reconnect back to HQ [2] .

Given the increasing success of artificial intelligence (AI) techniques in recent times and the advancements made in unmanned vehicle (UV) design, autonomous systems in particular teams or swarms of autonomous vehicles have become a potential attractive solution to solving the complexity issues facing the military today. Teams or swarms of intelligent autonomous vehicles would also introduce a level of survivability to the systems which would increase its utility in such complex and hostile environments [3] .

A problem which arises in utilizing large teams or swarms of autonomous vehicles is one of control. Recently, the US Defence Advanced Research Projects Agency (DARPA) has sought to address some of the control issues associated with large swarm through its Offensive Swarm-Enabled Tactics (OFFSET) [4] . Therefore, there is a clear need for both local and global control of large teams or swarms of UV. This would allow for autonomous or semi-autonomous operation of large teams of UV which would be easily manageable.

This paper proposes a Hamiltonian servo system which is a combined modeling framework for the control and estimation of such autonomous teams of UV’s. This paper will demonstrate the ability of this framework to handle the high-dimensional, nonlinear and non-Gaussian nature of the problem at hand. The authors will also show this to be a generalization of the classic Kalman Servo System. Finally results of a number of simulations will be presented to demonstrate the validity of the framework through the authors implementation using Mathematica & C++ code (which is provided in the Appendix).

Slightly more specifically, we recall here that traditional robotics from the 1970s-80s were mainly focused on building humanoid robots, manipulators and leg locomotion robots. Its modeling framework was linear dynamics and linear control, that is, linearized mechanics of multi-body systems (derived using Newton-Euler, Lagrangian, Gibs-Appel or kinetostatics equations of motion) and controlled by Kalman’s linear quadratic controllers (for a comprehensive review, see [5] - [11] ). The pinnacle of this approach to robotics in the last decade has been the famous Honda robot ASIMO (see [12] ), with a related Hamiltonian biomechanical simulator [13] .

In contrast, contemporary robotics research has mainly been focused on field robotics, based on estimation rather than control, resulting in a variety of self-localization and mapping (SLAM) algorithms (see [14] [15] [16] [17] [18] and the OpenSLAM web-site [19] ).

Instead of following one of these two sides of robotics, in the present paper we will try to combine them. From a robotics point-of-view, control and estimation are two complementary sides, or necessary components, of efficient manipulation of autonomous robotic vehicles. In this paper we attempt to develop a computational framework (designed in Wolfram Mathematica^Ò and implemented in C++) that unifies both of these components. From a mathematical point-of-view, in this paper we attempt to provide nonlinear, non-Gaussian and high-dimensional generalization of the classical Kalman servo system, outlined in the next section.

2. Motivation: Kalman Servo

In 1960s, Rudolf Kalman developed concept of a controller-estimator servo

Figure 1. Kalman’s fundamental controller-estimator servo system, including state-space dynamics, Kalman filter and feedback controller.

system (see Figure 1; for original Kalman’s work see [20] [21] [22] ), which consists of the following three main components, written in classical boldface matrix notation:

System state dynamics: linear, low-dimensional, multi-input multi-output process cascade (as implemented in Matlab^Ò Control and Signal Processing toolboxes):

$\stackrel{\dot{}}{x}=Ax+Bu\mathrm{,}y=Cx+Du\mathrm{,}\left[x\left(0\right)=x_0\right]\mathrm{,}$ (1)

where we have three vector spaces: state space $\mathbb{X}\subset {ℝ}^n$ , input space $\mathbb{U}\subset {ℝ}^m$ and output space $\mathbb{Y}\subset {ℝ}^k$ , such that $x\left(t\right)\in \mathbb{X}$ is an n-vector of state variables, $u\left(t\right)\in \mathbb{U}$ is an m-vector of inputs and $y\left(t\right)\in \mathbb{Y}$ is a k-vector of outputs; $x_0$ is the initial state vector. The matrix variables are the following maps: i) $n\times n$ dimensional state dynamics $A=A\left(t\right):\mathbb{X}\to \mathbb{X}$ ; ii) $n\times m$ dimensional input map $B=B\left(t\right)\mathrm{<mo>\; :\; </mo>}\mathbb{U}\to \mathbb{X}$ ; iii) $k\times n$ dimensional output map $C=C\left(t\right)\mathrm{<mo>\; :\; </mo>}\mathbb{X}\to \mathbb{Y}$ ; and (iv) $k\times m$ dimensional input-output transform $D=D\left(t\right)\mathrm{<mo>\; :\; </mo>}\mathbb{U}\to \mathbb{Y}$ . The first equation in (1) is called the state or dynamics equation and the second equation in (1) is called the output or measurement equation.

Kalman feedback controller: optimal LQR-controller:¹

$y=K\left[x_{ref}\left(t\right)-x\left(t\right)\right]\mathrm{,}$ (2)

with the controller gain matrix: $K=K\left(t\right)\mathrm{<mo>\; :\; </mo>}\mathbb{X}\to \mathbb{Y}=R_c^{-1}{B}^{\text{T}}{P}_c$ determined by the matrix $P_c$ governed by the controller Riccati ordinary differential equation (ODE):

${\stackrel{\dot{}}{P}}_c=A^{\text{T}}{P}_c+P_{c}A+Q_c-P_{c}B{R}_c^{-1}{B}^{\text{T}}{P}_c\mathrm{,}\left[P_c\left(0\right)=P_0^c\right]\mathrm{,}$

where $P_0^c$ is the initial controller matrix. $Q_c\mathrm{<mo>\; :\; </mo>}\mathbb{X}\to \mathbb{X}$ and $R_c\mathrm{<mo>\; :\; </mo>}\mathbb{U}\to \mathbb{U}$ are matrices from the quadratic cost function:

$J={\displaystyle \underset{t_0}{\overset{t_1}{\int }}}\left(x^{\text{T}}Qx+u^{\text{T}}Ru\right)\text{d}t\mathrm{.}$ (3)

Kalman filter: optimal LQG-estimator² with the (additive, zero-mean, delta- correlated) Gaussian white noise $\eta \left(t\right)$ :

$\stackrel{\dot{}}{x}=Ax+Bu+L\left[y-\left(Cx+Du\right)\right]+\eta \mathrm{,}\left[x\left(0\right)=x_0\right]\mathrm{,}$ (4)

with the filter gain matrix: $L=L\left(t\right):\mathbb{X}\to \mathbb{Y}=R_f^{-1}{P}_f{C}^{\text{T}}$ determined by the matrix $P_f$ governed by the filter Riccati ODE:

${\stackrel{\dot{}}{P}}_f=A^{\text{T}}{P}_f+P_{f}A+Q_f-P_f{C}^{\text{T}}{R}_f^{-1}C{P}_f\mathrm{,}\left[P_f\left(0\right)=P_0^f\right]\mathrm{,}$

where $P_0^f$ is the initial filter matrix. $Q_f\mathrm{<mo>\; :\; </mo>}\mathbb{X}\to \mathbb{X}$ and $R_f\mathrm{<mo>\; :\; </mo>}\mathbb{U}\to \mathbb{U}$ are covariance matrices from the noisy $\eta$ -generalization of the quadratic cost function (3).

Defined in this way, Kalman’s controller-estimator servo system has a two-cycle action, similar to the predictor-corrector algorithm: in the first time-step the servo controls the plant (e.g., a UGV), in the second step it estimates the plants ( $x\mathrm{,}y$ ) coordinates, in the next step it corrects the control, then estimates again, and so on.

In the remainder of this paper we will develop a nonlinear, non-Gaussian and high-dimensional generalization of the Kalman servo.

3. Control of an Autonomous Team of UGVs: Affine Hamiltonian Neural Network

Now we switch from matrix to ( $\alpha ,\beta =1,\cdots ,n$ )-index notation,³ to label the position of $n\times n$ individual UGVs within the swarm’s global plane coordinates, longitude and latitude, respectively. The first step in the nonlinear generalization of the Kalman servo is replacing the linear, low-dimensional, state dynamics (1) and control (2)-(3) with nonlinear, adaptive and high-dimensional dynamics and control system, as follows.

In the recent paper [23] , we have proposed a dynamics and control model for a joint autonomous land-air operation, consisting of a swarm of unmanned ground vehicles (UGVs) and swarm of unmanned aerial vehicles (UAVs). In the present paper we are focusing on a swarm of UGVs only, from both control and estimation perspectives. It can be modeled as an affine Hamiltonian control system (see [24] ) with many degrees-of-freedom (DOFs), reshaped in the form of a ( $q\mathrm{,}p$ )-pair of attractor matrix equations with nearest-neighbor couplings defined by the matrix of affine Hamiltonians:

$H_{\alpha \beta }=\frac{1}{2}\left(q_{\alpha \beta }{q}_{\alpha -1\beta -1}+p_{\alpha \beta }{p}_{\alpha -1\beta -1}\right).$

The following pair of attractor matrix equations defines the activation dynamics of a bidirectional recurrent neural network:

${\stackrel{\dot{}}{q}}_{\alpha \beta }=\phi \left(A^q-q_{\alpha \beta }-{\omega }_{\alpha \beta }^q{q}_{\alpha \beta }^2{p}_{\alpha \beta }\right)-{\displaystyle \underset{j\mathrm{,}k}{\sum }}{\partial }_{p_{\alpha \beta }}{H}_{\alpha \beta }{u}_{\alpha \beta }^q\mathrm{,}$ (5)

${\stackrel{\dot{}}{p}}_{\alpha \beta }=\phi \left(A^p-p_{\alpha \beta }-{\omega }_{\alpha \beta }^p{p}_{\alpha \beta }^2{q}_{\alpha \beta }\right)+{\displaystyle \underset{j\mathrm{,}k}{\sum }}{\partial }_{q_{\alpha \beta }}{H}_{\alpha \beta }{u}_{\alpha \beta }^p\mathrm{,}$ (6)

where superscripts ( $q\mathrm{,}p$ ) denote the corresponding Hamiltonian equations and the partial derivative is written as: ${\partial }_{u}H\equiv \partial H/\partial u$ . In Equations ((5), (6)) the matrices $q_{\alpha \beta }=q_{\alpha \beta }\left(t\right)$ and $p_{\alpha \beta }=p_{\alpha \beta }\left(t\right)$ define time evolution of the swarm’s coordinates and momenta, respectively, with initial conditions: $q_{\alpha \beta }\left(0\right)$ and $p_{\alpha \beta }\left(0\right)$ . The field attractor lines $A^q$ and $A^p$ (with the field strength $\phi$ ) are defined in the simulated urban environment using the A-star algorithm that finds the shortest Manhattan distance from point A to point B on the environmental digital map (i.e., street directory). The adaptive synaptic weights matrices: ${\omega }_{\alpha \beta }^q={\omega }_{\alpha \beta }^q\left(t\right)$ and ${\omega }_{\alpha \beta }^p={\omega }_{\alpha \beta }^p\left(t\right)$ are trained by Hebbian learning (11) defined below. Input matrices $u_{\alpha \beta }^q=u_{\alpha \beta }^q\left(t\right)$ and $u_{\alpha \beta }^p=u_{\alpha \beta }^p\left(t\right)$ are Lie-derivative based controllers (explained below).

Equations ((5), (6)) are briefly derived as follows (for technical details, see [23] and the references therein). Given the swarm configuration manifold M (as a product of Euclidean groups of motion for each robotic vehicle), the affine Hamiltonian function $H_a\left(q\mathrm{,}p\mathrm{,}u\right)\mathrm{<mo>\; :\; </mo>}{T}_{\ast }M\to ℝ$ is defined on its cotangent bundle $T_{\ast }M$ by (see [24] ):

$H_a\left(q,p,u\right)=H_0\left(q,p\right)-{\displaystyle \underset{j<n}{\sum }}{H}_j\left(q,p\right)u_j\left(t,q,p\right),$ (7)

^{4 $c_{s-1}$} are the coefficients of the linear ODE of order r for the error function $e\left(t\right)=q_j\left(t\right)-q_j^{\text{ref}}\left(t\right)$ :

$e^{\left(r\right)}+c_{r-1}{e}^{\left(r-1\right)}+\cdots +c_1{e}^{\left(1\right)}+c_{0}e=0.$

where $H_0\left(q\mathrm{,}p\right)\mathrm{<mo>\; :\; </mo>}{T}_{\ast }M\to ℝ$ is the physical Hamiltonian function (kinetic plus potential energy of the swarm) and control inputs $u_j\left(t\mathrm{,}q\mathrm{,}p\right)$ are defined using recurrent Lie derivatives:⁴

$u_j=\frac{{\stackrel{\dot{}}{o}}_R^{\left(r\right)j}-L_f^{\left(r\right)}{H}_j+{\displaystyle \underset{s=1}{\overset{r}{\sum }}}{c}_{s-1}\left(o_R^{\left(s-1\right)j}-L_f^{\left(s-1\right)}{H}_j\right)}{L_g{L}_f^{\left(r-1\right)}{H}_j}.$ (8)

Using (7) and (8), the affine Hamiltonian control system can be formulated (for $i=1,\cdots ,n;j<i$ ) as:

${\stackrel{\dot{}}{q}}_i=V_i+{\partial }_{p_i}{H}_0-{\displaystyle \underset{j<i}{\sum }}{\partial }_{p_i}{H}_j{u}_j+{\partial }_{p_i}D,$ (9)

${\stackrel{\dot{}}{p}}_i=F_i-{\partial }_{q_i}{H}_0+{\displaystyle \underset{j<i}{\sum }}{\partial }_{q_i}{H}_j{u}_j+{\partial }_{q_i}D.$ (10)

where $D$ is Rayleigh’s dissipative function and $V_i=V_i\left(t,q,p\right)$ and $F_i=$ $F_i\left(t,q,p\right)$ are velocity and force controllers, respectively.

The affine Hamiltonian control system (9)-(10) can be reshaped into a matrix form suitable for a UGV-swarm or a planar formation of a UAV-swarm: $q_i\to q_{\alpha \beta }$ , $p_i\to p_{\alpha \beta }$ , $u_i\to u_{\alpha \beta }$ , by evaluating dissipation terms into cubic terms in the brackets, and replacing velocity and force controllers with attractors $A^q$ and $A^p$ , respectively (with strength $\phi$ ). If we add synaptic weights ${\omega }_{\alpha \beta }$ we come to our recurrent neural network Equations ((5), (6)).

Next, to make Equations ((5), (6)) adaptive, we use the abstract Hebbian learning scheme:

$\text{NewValue}=\text{OldValue}+\text{Innovation}$

which in our settings formalizes as (see [25] ):

${\stackrel{\dot{}}{\omega }}_{\alpha \beta }^q=-{\omega }_{\alpha \beta }^q+{\Phi }_{\alpha \beta }^q\left(q_{\alpha \beta },p_{\alpha \beta }\right),$ (11)

${\stackrel{\dot{}}{\omega }}_{\alpha \beta }^p=-{\omega }_{\alpha \beta }^p+{\Phi }_{\alpha \beta }^p\left(q_{\alpha \beta },p_{\alpha \beta }\right),$

where the innovations are given in the matrix signal form (generalized from [26] ):

${\Phi }_{\alpha \beta }^q=S_{\alpha \beta }^q\left(q_{\alpha \beta }\right)S_{\alpha \beta }^q\left(p_{\alpha \beta }\right)+{\stackrel{\dot{}}{S}}_{\alpha \beta }^q\left(q_{\alpha \beta }\right){\stackrel{\dot{}}{S}}_{\alpha \beta }^q\left(p_{\alpha \beta }\right),$ (12)

${\Phi }_{\alpha \beta }^p=S_{\alpha \beta }^p\left(q_{\alpha \beta }\right)S_{\alpha \beta }^p\left(p_{\alpha \beta }\right)+{\stackrel{\dot{}}{S}}_{\alpha \beta }^p\left(q_{\alpha \beta }\right){\stackrel{\dot{}}{S}}_{\alpha \beta }^p\left(p_{\alpha \beta }\right),$

with sigmoid activation functions $S(\cdot )=tanh(\cdot )$ and signal velocities: $\stackrel{\dot{}}{S}(\cdot )=$ $1-tanh(\cdot )$ .

In this way defined affine Hamiltonian control system (5)-(6), which is also a recurrent neural network with Hebbian leaning (11)-(12), represents our nonlinear, adaptive and high-dimensional generalization of Kalman’s linear state dynamics (1) and control (2)-(3).

Using Wolfram Mathematica^Ò code given below, the Hamiltonian neural net was simulated for 1 sec, with the matrix dimensions $n=10$ (i.e., with 100 neurons in both matrices $q_{ij}$ and $p_{ij}$ , which are longitudes and latitudes of a large fleet of 100 UGVs, see Figures 2-4). The Figures illustrate both stability and convergence of the recurrent Hamiltonian neural net.

C++ code for the the Hamiltonian neural net is given in Appendix 1.1.

Mathematica^Ò code

Dimensions:

$n=\mathrm{10\; <mo>\; ;\; </mo>}\text{Tfin}=\mathrm{1\; <mo>\; ;\; </mo>}\phi =\mathrm{30\; <mo>\; ;\; </mo>}$

Coordinates and momenta:

Figure 2. Adaptive control for a large fleet (a $10\times 10$ matrix) of 100 UGVs: time evolution of coordinates $q_{\alpha \beta }\left(t\right)$ , where $\alpha ,\beta =1,\cdots ,10$ . As expected, we can see the exponential-type growth of coordinates $q_{\alpha \beta }\left(t\right)$ together with their spreading from zero initial conditions. This plot is the simulation output of the recurrent activation dynamics given by Equations ((5), (6)) with Hebbian learning dynamics given by Equations ((11), (12)), starting from zero initial coordinates and momenta and random $\left(-\mathrm{1,1}\right)$ initial weights.

Figure 3. Adaptive control for a large fleet (a $10\times 10$ matrix) of 100 UGVs: time evolution of momenta $p_{\alpha \beta }\left(t\right)$ . As expected, we can see the exponential-type decay of momenta $p_{\alpha \beta }\left(t\right)$ together with their spreading, after the initial convergent growth from zero initial conditions. This plot is the simulation output from the same recurrent neural network as in Figure 2.

Figure 4. Adaptive control for a large fleet (a $10\times 10$ matrix) of 100 UGVs: time evolution of synaptic weights ${\omega }_{\alpha \beta }\left(t\right)$ . As expected, we can see the converging behavior of the weights ${\omega }_{\alpha \beta }\left(t\right)$ , starting from their initial random distribution. Simulating the same recurrent neural network from Figure 2.

$\begin{array}{l}\mathrm{Table}\left[q_{\alpha \mathrm{,}\beta }\left[t\right]\mathrm{,}\left\{\alpha \mathrm{,}n\right\}\mathrm{,}\left\{\beta \mathrm{,}n\right\}\right]\mathrm{<mo>\; ;\; </mo>}\\ \mathrm{Table}\left[p_{\alpha \mathrm{,}\beta }\left[t\right]\mathrm{,}\left\{\alpha \mathrm{,}n\right\}\mathrm{,}\left\{\beta \mathrm{,}n\right\}\right]\mathrm{<mo>\; ;\; </mo>}\end{array}$

Hyperbolic tangent activation functions:

$\begin{array}{l}\mathrm{Table}\left[{\text{Sq}}_{\alpha \mathrm{,}\beta }=\text{Tanh}\left[q_{\alpha \mathrm{,}\beta }\left[t\right]\right]\mathrm{,}\left\{\alpha \mathrm{,}n\right\}\mathrm{,}\left\{\beta \mathrm{,}n\right\}\right]\mathrm{<mo>\; ;\; </mo>}\\ \mathrm{Table}\left[{\text{Sp}}_{\alpha \mathrm{,}\beta }=\text{Tanh}\left[p_{\alpha \mathrm{,}\beta }\left[t\right]\right]\mathrm{,}\left\{\alpha \mathrm{,}n\right\}\mathrm{,}\left\{\beta \mathrm{,}n\right\}\right]\mathrm{<mo>\; ;\; </mo>}\end{array}$

Derivatives of activation functions:

$\begin{array}{l}\mathrm{Table}\left[{\text{Sdq}}_{\alpha \mathrm{,}\beta }=1-{\text{Sq}}_{\alpha \mathrm{,}\beta }^2\mathrm{,}\left\{\alpha \mathrm{,}n\right\}\mathrm{,}\left\{\beta \mathrm{,}n\right\}\right]\mathrm{<mo>\; ;\; </mo>}\\ \mathrm{Table}\left[{\text{Sdp}}_{\alpha \mathrm{,}\beta }=1-{\text{Sp}}_{\alpha \mathrm{,}\beta }^2\mathrm{,}\left\{\alpha \mathrm{,}n\right\}\mathrm{,}\left\{\beta \mathrm{,}n\right\}\right]\mathrm{<mo>\; ;\; </mo>}\end{array}$

Control inputs:

$\mathrm{Table}\left[a_{\alpha \mathrm{,}\beta }=\text{RandomReal}\left[\left\{-\mathrm{1,1}\right\}\right]\mathrm{,}\left\{\alpha \mathrm{,}n\right\}\mathrm{,}\left\{\beta \mathrm{,}n\right\}\right]\mathrm{<mo>\; ;\; </mo>}$

$\mathrm{Table}\left[b_{\alpha \mathrm{,}\beta }=\text{RandomReal}\left[\left\{-\mathrm{1,1}\right\}\right]\mathrm{,}\left\{\alpha \mathrm{,}n\right\}\mathrm{,}\left\{\beta \mathrm{,}n\right\}\right]\mathrm{<mo>\; ;\; </mo>}$

$\mathrm{Table}\left[c_{\alpha \mathrm{,}\beta }=\text{RandomReal}\left[\left\{-\mathrm{1,1}\right\}\right]\mathrm{,}\left\{\alpha \mathrm{,}n\right\}\mathrm{,}\left\{\beta \mathrm{,}n\right\}\right]\mathrm{<mo>\; ;\; </mo>}$

$\mathrm{Table}\left[u_{\alpha \mathrm{,}\beta }\mathrm{<mo\; stretchy="false">\; [\; </mo>}t\mathrm{<mo\; stretchy="false">\; ]\; </mo>}=a_{\alpha \mathrm{,}\beta }\text{Sin}\left[b_{\alpha \mathrm{,}\beta }+c_{\alpha \mathrm{,}\beta }t\right]\mathrm{,}\left\{\alpha \mathrm{,}n\right\}\mathrm{,}\left\{\beta \mathrm{,}n\right\}\right]\mathrm{<mo>\; ;\; </mo>}$

Affine Hamiltonians (nearest-neighbor coupling):

$\mathrm{Table}\left[H_{\alpha \mathrm{,}\beta }=\frac{1}{2}\left(q_{\alpha \mathrm{,}\beta }\left[t\right]q_{\alpha -\mathrm{1,}\beta -1}\left[t\right]+p_{\alpha \mathrm{,}\beta }\left[t\right]p_{\alpha -\mathrm{1,}\beta -1}\left[t\right]\right)\mathrm{,}\left\{\alpha \mathrm{,2,}n\right\}\mathrm{,}\left\{\beta \mathrm{,2,}n\right\}\right]\mathrm{<mo>\; ;\; </mo>}$

Attractor lines (figure-8 attractor):

$\text{Aq}=\text{Sin}\left[t\right]\mathrm{<mo>\; ;\; </mo>}\text{Ap}=\text{Cos}\left[t\right]\mathrm{<mo>\; ;\; </mo>}$

Equations of motion:

$\begin{array}{l}\text{Eqns}=\text{Flatten}[\text{Join}[\\ \mathrm{Table}[\{{q^{\prime }}_{\alpha \mathrm{,}\beta }\left[t\right]==\phi \left(\text{Aq}-q_{\alpha \mathrm{,}\beta }\left[t\right]-{\omega }_{\alpha \mathrm{,}\beta }\left[t\right]q_{\alpha \mathrm{,}\beta }{\left[t\right]}^2{p}_{\alpha \mathrm{,}\beta }\left[t\right]\right)\\ -{\displaystyle {\sum }_{\alpha =1}^n}{\displaystyle {\sum }_{\beta =1}^n}{\partial }_{p_{\alpha \mathrm{,}\beta }\left[t\right]}{H}_{\alpha \mathrm{,}\beta }{u}_{\alpha \mathrm{,}\beta }\left[t\right]\mathrm{,}{q}_{\alpha \mathrm{,}\beta }\left[0\right]==0\}\mathrm{,}\left\{\alpha \mathrm{,}n\right\}\mathrm{,}\left\{\beta \mathrm{,}n\right\}]\mathrm{,}\\ \mathrm{Table}[\{{p^{\prime }}_{\alpha \mathrm{,}\beta }\left[t\right]==\phi \left(\text{Ap}-p_{\alpha \mathrm{,}\beta }\left[t\right]-{\omega }_{\alpha \mathrm{,}\beta }\left[t\right]p_{\alpha \mathrm{,}\beta }{\left[t\right]}^2{q}_{\alpha \mathrm{,}\beta }\left[t\right]\right)\\ +{\displaystyle {\sum }_{\alpha =1}^n}{\displaystyle {\sum }_{\beta =1}^n}{\partial }_{q_{\alpha \mathrm{,}\beta }\left[t\right]}{H}_{\alpha \mathrm{,}\beta }{u}_{\alpha \mathrm{,}\beta }\left[t\right]\mathrm{,}{p}_{\alpha \mathrm{,}\beta }\left[0\right]==0\}\mathrm{,}\left\{\alpha \mathrm{,}n\right\}\mathrm{,}\left\{\beta \mathrm{,}n\right\}]\mathrm{,}\end{array}$

$\begin{array}{l}\mathrm{Table}[\{{{\omega }^{\prime }}_{\alpha \mathrm{,}\beta }\left[t\right]==-{\omega }_{\alpha \mathrm{,}\beta }\left[t\right]+{\text{Sq}}_{\alpha \mathrm{,}\beta }{\text{Sp}}_{\alpha \mathrm{,}\beta }+{\text{Sdq}}_{\alpha \mathrm{,}\beta }{\text{Sdp}}_{\alpha \mathrm{,}\beta }\mathrm{,}\\ {\omega }_{\alpha \mathrm{,}\beta }\left[0\right]==\text{RandomReal}\left[\left\{-\mathrm{1,1}\right\}\right]\}\mathrm{,}\left\{\alpha \mathrm{,}n\right\}\mathrm{,}\left\{\beta \mathrm{,}n\right\}]]]\mathrm{<mo>\; ;\; </mo>}\end{array}$

Numerical solution:

$\begin{array}{l}\text{sol}=\text{NDSolve}\mathrm{<mo\; stretchy="false">\; [\; </mo>}[\text{Eqns}\mathrm{,}\text{Flatten}[\text{Join}[\mathrm{Table}\left[q_{\alpha \mathrm{,}\beta }\mathrm{,}\left\{\alpha \mathrm{,}n\right\}\mathrm{,}\left\{\beta \mathrm{,}n\right\}\right]\mathrm{,}\\ \mathrm{Table}\left[p_{\alpha \mathrm{,}\beta }\mathrm{,}\left\{\alpha \mathrm{,}n\right\}\mathrm{,}\left\{\beta \mathrm{,}n\right\}\right]\mathrm{,}\mathrm{Table}\left[{\omega }_{\alpha \mathrm{,}\beta }\mathrm{,}\left\{\alpha \mathrm{,}n\right\}\mathrm{,}\left\{\beta \mathrm{,}n\right\}\right]]]\mathrm{,}\left\{t\mathrm{,0,}\text{Tfin}\right\}]\mathrm{<mo>\; ;\; </mo>}\end{array}$

Plots of coordinates:

$\begin{array}{l}\text{Plot}[\text{Evaluate}\left[\mathrm{Table}\left[q_{\alpha \mathrm{,}\beta }\left[t\right]\text{/}\text{.sol}\mathrm{,}\left\{\alpha \mathrm{,}n\right\}\mathrm{,}\left\{\beta \mathrm{,}n\right\}\right]\mathrm{,}\left\{t\mathrm{,0,}\text{Tfin}\right\}\right]\mathrm{,}\\ \text{PlotRange}\to \text{All}\mathrm{,}\text{PlotStyle}\to \text{AbsoluteThickness}\left[1.5\right]\mathrm{,}\text{Frame}\to \text{True}]\end{array}$

Plots of momenta:

$\begin{array}{l}\text{Plot}[\text{Evaluate}\left[\mathrm{Table}\left[p_{\alpha \mathrm{,}\beta }\left[t\right]\text{/}\text{.sol}\mathrm{,}\left\{\alpha \mathrm{,}n\right\}\mathrm{,}\left\{\beta \mathrm{,}n\right\}\right]\mathrm{,}\left\{t\mathrm{,0,}\text{Tfin}\right\}\right]\mathrm{,}\\ \text{PlotRange}\to \text{All}\mathrm{,}\text{PlotStyle}\to \text{AbsoluteThickness}\left[1.5\right]\mathrm{,}\text{Frame}\to \text{True}]\end{array}$

Plots of weights:

$\begin{array}{l}\text{Plot}[\text{Evaluate}\left[\mathrm{Table}\left[{\omega }_{\alpha \mathrm{,}\beta }\left[t\right]\text{/}\text{.sol}\mathrm{,}\left\{\alpha \mathrm{,}n\right\}\mathrm{,}\left\{\beta \mathrm{,}n\right\}\right]\mathrm{,}\left\{t\mathrm{,0,}\text{Tfin}\right\}\right]\mathrm{,}\\ \text{PlotRange}\to \text{All}\mathrm{,}\text{PlotStyle}\to \text{AbsoluteThickness}\left[1.5\right]\mathrm{,}\text{Frame}\to \text{True}]\end{array}$

4. Estimation of an Autonomous Team of UGVs: High-Dimensional Bayesian Particle Filter

The second step in the nonlinear generalization of the Kalman servo is to replace the linear/Gaussian Kalman filter (4) with a general nonlinear/non-Gaussian Monte-Carlo filter, as follows.

4.1. Recursive Bayesian Filter

Recall that the celebrated Bayes rule gives the relation between the three basic conditional probabilities: i) a Prior $P\left(A\right)$ (i.e., an initial degree-of-belief in event A, that is, Initial Odds), ii) Likelihood $P\left(B\mathrm{<mo>\; |\; </mo>}A\right)$ (i.e., a degree-of-belief in event B, given A, that is, a New Evidence); and Posterior: $P\left(A\mathrm{<mo>\; |\; </mo>}B\right)$ (i.e., a degree-of-belief in A, given B, that is, New Odds). Provided $P\left(B\right)\ne 0$ , Bayes rule reads:

$\stackrel{\text{Posterior}}{P\left(A\mathrm{<mo>\; |\; </mo>}B\right)}=\frac{\stackrel{\text{Prior}}{P\left(A\right)}\times \stackrel{\text{Likelihood}}{P\left(B\mathrm{<mo>\; |\; </mo>}A\right)}}{P\left(B\right)}\mathrm{.}$ (13)

In statistical settings, Bayes rule (13) can be rewritten as:

$\stackrel{\text{Posterior}}{p\left(H\mathrm{<mo>\; |\; </mo>}D\right)}=\frac{\stackrel{\text{Prior}}{p\left(H\right)}\times \stackrel{\text{Likelihood}}{p\left(D\mathrm{<mo>\; |\; </mo>}H\right)}}{p\left(D\right)}\mathrm{,}$ (14)

where $p\left(H\right)$ is the prior probability density function (PDF) that the hypothesis H is true, $p\left(D\mathrm{<mo>\; |\; </mo>}H\right)$ is the likelhood PDF for the data D given a hypothesis H, $p\left(H\mathrm{<mo>\; |\; </mo>}D\right)$ is the posterior PDF that the hypothesis H is true given the data D, and the normalization constant: $p\left(D\right)\mathrm{<mo>\; =\; </mo>}{\displaystyle {\sum }_{H_i}}p\left(D\mathrm{<mo>\; |\; </mo>}H\right)p\left(H\right)$ is the PDF for the data D averaged over all possible hypotheses $H_i$ .

When Bayes rule (13)-(14) is applied iteratively/recursively over signal distributions evolving in discrete time steps, in such a way that the Old Posterior becomes the New Prior and New Evidence is added, it becomes the recursive Bayesian filter, the formal origin of all Kalman and particle filters. The Bayesian filter can be applied to estimate the hidden state $x_t$ of a nonlinear dynamical system evolving in discrete time steps (e.g., a numerical solution of a system of ODEs, or discrete-time sampling measurements) in a recursive manner by processing a sequence of observations $Y_T={\left\{y_t\right\}}_{t=1}^T$ dependent on the state $x_t$ within a dynamic noise (either Gaussian, or non-Gaussian) ${\eta }_t$ measured as ${\mu }_t$ . The state dynamics and measurement, or the so-called state-space model (SSM) are usually given either by Kalman’s state Equation (1), or by its nonlinear generalization. Instead of the Kalman filter Equation (4) combined with the cost function (3), the Bayesian filter includes (see, e.g. [27] [28] ):

Time-update equation:

$\stackrel{\text{Predictor}}{p\left(x_t\mathrm{<mo>\; |\; </mo>}{Y}_{t-1}\right)}={\displaystyle {\int }_{{ℝ}^n}}\stackrel{\text{Prior}}{p\left(x_t\mathrm{<mo>\; |\; </mo>}{x}_{t-1}\right)}\stackrel{\text{Old}\text{Posterior}}{p\left(x_{t-1}\mathrm{<mo>\; |\; </mo>}{Y}_{t-1}\right)}\text{d}{x}_{t-1}\mathrm{,}$ (15)

and

Measurement-update equation:

$\stackrel{\text{New}\text{Posterior}}{p\left(x_t\mathrm{<mo>\; |\; </mo>}{Y}_t\right)}=Z_t^{\left(-1\right)}\stackrel{\text{Predictor}}{p\left(x_t\mathrm{<mo>\; |\; </mo>}{Y}_{t-1}\right)}\stackrel{\text{Likelihood}}{l\left(y_t\mathrm{<mo>\; |\; </mo>}{x}_t\right)}\mathrm{,}$ (16)

where $Z_t$ is the normalizing constant, or partition function, defined by:

$Z_t={\displaystyle {\int }_{{ℝ}^n}}p\left(x_t\mathrm{<mo>\; |\; </mo>}{Y}_{t-1}\right)l\left(y_t\mathrm{<mo>\; |\; </mo>}{x}_t\right)\text{d}{x}_t\mathrm{.}$

In the Bayesian filter (15)-(16), given the sequence of observations $Y_t$ , the posterior fully defines the available information about the state of the system and the noisy environment―the filter propagates the posterior (embodying time and measurement updates for each iteration) across the nonlinear state-space model; therefore, it is the maximum a posteriori estimator of the system state. In a special case of a linear system and Gaussian environment, the filter (15)-(16) has optimal closed-form solution, which is the Kalman filter (4). However, in the general case of a nonlinear dynamics and/or the non-Gaussian environment, it is no longer feasible to search for closed-form solutions for the integrals in (15)-(16), so we are left with suboptimal, approximate, Monte Carlo solutions, called particle filters.

4.2. Sequential Monte Carlo Particle Filter

Now we consider a general, discrete-time, nonlinear, probabilistic SSM, defined by a Markov process ${\left\{x_t\right\}}_{t\ge 1}\subseteq {ℝ}^{n_x}$ , which is observed indirectly via a measurement process ${\left\{y_t\right\}}_{t\ge 1}\subseteq {ℝ}^{n_y}$ . This SSM consists of two probability density functions: dynamics $f(\cdot )$ and measurement $h(\cdot )$ . Formally, we have a system of nonlinear difference equations, given both in Bayesian probabilistic formulation (left-hand side) and nonlinear state-space formulation (right-hand side):

$\text{Dynamics}\mathrm{<mo>\; :\; </mo>\; <mi>\; </mi>\; <mi>\; </mi>}{x}_{t+1}\mathrm{<mo>\; |\; </mo>}{x}_t~f\left(x_{t+1}\mathrm{<mo>\; |\; </mo>}{x}_t\right)\iff x_{t+1}=f\mathrm{<mo\; stretchy="false">\; (\; </mo>}{x}_t\mathrm{<mo\; stretchy="false">\; )\; </mo>}+{ϵ}_t\mathrm{,}$ (17)

$\text{Measurement}\mathrm{<mo>\; :\; </mo>\; <mi>\; </mi>}{y}_t\mathrm{<mo>\; |\; </mo>}{x}_t~h\left(y_t\mathrm{<mo>\; |\; </mo>}{x}_t\right)\iff y_t=h\left(x_t\right)+{\nu }_t\mathrm{,}$ (18)

$\text{withinitialstate}\mathrm{<mo>\; :\; </mo>\; <mi>\; </mi>\; <mi>\; </mi>}{x}_1~\mu \left(x_1\right)\mathrm{,}$

where $f$ and $h$ are the state and output vector-functions, ${ϵ}_t$ and ${\nu }_t$ are non-Gaussian process and measurement noises and $\mu$ is a given non-Gaussian distribution. The state filtering problem means to recover information about the current state $x_t$ in (17) based on the available measurements $y_{\mathrm{1\; <mo>\; :\; </mo>}t}$ in (18) (see, e.g. [29] ; for a recent review, see [30] and the references therein).

The principle solution to the nonlinear/non-Gaussian state filtering problem is provided by the following recursive Bayesian measurement and time update equations:

$\stackrel{\text{New}\text{Filter}}{p\left(x_t\mathrm{<mo>\; |\; </mo>}{y}_{\mathrm{1\; <mo>\; :\; </mo>}t}\right)}=\mathrm{<mi>\; </mi>}\frac{\stackrel{\text{Measure}}{h\left(y_t\mathrm{<mo>\; |\; </mo>}{x}_t\right)}\stackrel{\text{Predictor}}{p\left(x_t\mathrm{<mo>\; |\; </mo>}{y}_{\mathrm{1\; <mo>\; :\; </mo>}t-1}\right)}}{p\left(y_t\mathrm{<mo>\; |\; </mo>}{y}_{\mathrm{1\; <mo>\; :\; </mo>}t-1}\right)}\mathrm{,}$ (19)

where

$\stackrel{\text{Predictor}}{p\left(x_t\mathrm{<mo>\; |\; </mo>}{y}_{\mathrm{1\; <mo>\; :\; </mo>}t-1}\right)}={\displaystyle \int }\stackrel{\text{Dynamics}}{f\left(x_t\mathrm{<mo>\; |\; </mo>}{x}_{t-1}\right)}\stackrel{\text{Old}\text{Filter}}{p\left(x_{t-1}\mathrm{<mo>\; |\; </mo>}{y}_{\mathrm{1\; <mo>\; :\; </mo>}t-1}\right)}\text{d}{x}_{t-1}.$ (20)

In a particular case of linear/Gaussian SSMs (17)-(18), the state filtering problem (19)-(20) has an optimal closed-form solution for the filter PDF $p\left(x_t\mathrm{<mo>\; |\; </mo>}{y}_{\mathrm{1\; <mo>\; :\; </mo>}t}\right)$ , given by the Kalman filter (4). However, in the general case of nonlinear/non-Gaussian SSMs―such an optimal closed-form solution does not exist.

The general state filtering problem (19)-(20) associated with any nonlinear/ non-Gaussian SSMs (17)-(18) can be only numerically approximated using the sequential Monte Carlo (SMC) methods. The SMC-approximation to the filter PDF, denoted by $\hat{p}\left(x_t\mathrm{<mo>\; |\; </mo>}{y}_{\mathrm{1\; <mo>\; :\; </mo>}t}\right)$ , is an empirical distribution represented as a weighted sum of Dirac-delta functions:

$\hat{p}\left(x_t|y_{1:t}\right)={\displaystyle \underset{i=1}{\overset{N}{\sum }}}{w}_t^i{\delta }_{x_t^i}\left(x_t\right),$ (21)

where the samples ${\left\{x_t^i\right\}}_{i=1}^N$ are called particles (point-masses ‘spread out’ in the state space); each particle $x_t^i$ represents one possible system state and the corresponding weight $w_t^i$ assigns its probability. In this way defined particle filter (PF) plays the role of the Kalman filter for nonlinear/non-Gaussian SSMs. PF approximates the filter PDF $p\left(x_t\mathrm{<mo>\; |\; </mo>}{y}_{\mathrm{1\; <mo>\; :\; </mo>}t}\right)$ using the SMC delta-approximation (21).

Briefly, a PF can be interpreted as a sequential application of the SMC technique called importance sampling (IS, see e.g. [31] ). Starting from the initial approximation: $\hat{p}\left(x_1\mathrm{<mo>\; |\; </mo>}{y}_1\right)\propto h\left(y_1\mathrm{<mo>\; |\; </mo>}{x}_1\right)\mu \left(x_1\right)$ , at each time step the IS is used to approximate the filter PDF $p\left(x_t\mathrm{<mo>\; |\; </mo>}{y}_{\mathrm{1\; <mo>\; :\; </mo>}t}\right)$ by using the recursive Bayesian Equations ((19), (20)) together with the already generated IS approximation of $p\left(x_{t-1}\mathrm{<mo>\; |\; </mo>}{y}_{\mathrm{1\; <mo>\; :\; </mo>}t-1}\right)$ . The particles ${\left\{x_1^i\right\}}_{i=1}^N$ are sampled independently from some proposal distribution $r\left(x_1\right)$ . To account for the discrepancy between the proposal distribution and the target distribution, the particles are assigned importance weights, given by the ratio between the target and the proposal: $w_1^i\propto h\left(y_1\mathrm{<mo>\; |\; </mo>}{x}_1^i\right)\mu \left(x_1^i\right)/r\left(x_1^i\right)$ , where the weights are normalized to sum to one (for a recent technical review, see e.g., [30] ).

The IS proceeds in an inductive fashion:

$\hat{p}\left(x_{t-1}\mathrm{<mo>\; |\; </mo>}{y}_{\mathrm{1\; <mo>\; :\; </mo>}t-1}\right)={\displaystyle \underset{i=1}{\overset{N}{\sum }}}{w}_{t-1}^i{\delta }_{x_{t-1}^i}\left(x_{t-1}\right)\mathrm{,}$

which, inserted as an old/previous filter into the time-update Equation (20), gives a mixture distribution, approximating $p\left(x_t\mathrm{<mo>\; |\; </mo>}{y}_{\mathrm{1\; <mo>\; :\; </mo>}t-1}\right)$ as:

$\hat{p}\left(x_t\mathrm{<mo>\; |\; </mo>}{y}_{\mathrm{1\; <mo>\; :\; </mo>}t-1}\right)={\displaystyle \int }\stackrel{\text{Dynamics}}{f\left(x_t\mathrm{<mo>\; |\; </mo>}{x}_{t-1}\right)}{\displaystyle \underset{i=1}{\overset{N}{\sum }}}{w}_{t-1}^i{\delta }_{x_{t-1}^i}\left(x_{t-1}\right)\text{d}{x}_{t-1}={\displaystyle \underset{i=1}{\overset{N}{\sum }}}{w}_{t-1}^{i}f\left(x_t|x_{t-1}^i\right)\mathrm{.}$ (22)

Subsequent insertion of (22) as a predictor into the measurement-update Equation (19), gives the following approximation of the filter PDF:

$\stackrel{\text{Filter}}{p\left(x_t\mathrm{<mo>\; |\; </mo>}{y}_{\mathrm{1\; <mo>\; :\; </mo>}t}\right)}\approx \frac{\stackrel{\text{Measure}}{h\left(y_t\mathrm{<mo>\; |\; </mo>}{x}_t\right)}}{p\left(y_t\mathrm{<mo>\; |\; </mo>}{y}_{\mathrm{1\; <mo>\; :\; </mo>}t-1}\right)}{\displaystyle \underset{i=1}{\overset{N}{\sum }}}\stackrel{\text{Weighted}\text{Dynamics}}{w_{t-1}^{i}f\left(x_t\mathrm{<mo>\; |\; </mo>}{x}_{t-1}^i\right)}\mathrm{,}$

which needs to be further approximated using the IS. The proposal density is pragmatically chosen as: $r\left(x_t\mathrm{<mo>\; |\; </mo>}{y}_{\mathrm{1\; <mo>\; :\; </mo>}t}\right)={\displaystyle {\sum }_{j=1}^N}{w}_{t-1}^{j}f\left(x_t\mathrm{<mo>\; |\; </mo>}{x}_{t-1}^j\right)$ . The N ancestor indices ${\left\{a_t^i\right\}}_{i=1}^N$ are resampled into a new set of particles ${\left\{a_{t-1}^i\right\}}_{i=1}^N$ which is subsequently used to propagate the particles to time t.

The final step is to assign the importance weights to the new particles as:

${\bar{w}}_t^i=\frac{h\left(y_t|x_t^i\right){\displaystyle \underset{j=1}{\overset{N}{\sum }}}{w}_{t-1}^{j}f\left(x_t^i|x_{t-1}^j\right)}{{\displaystyle \underset{j=1}{\overset{N}{\sum }}}{w}_{t-1}^{j}f\left(x_t|x_{t-1}^j\right)}.$

By evaluating ${\bar{w}}_t^i$ for $i=1,\cdots ,N$ and normalizing the weights, we obtain a new set of weighted particles ${\left\{x_t^i\mathrm{,}{w}_t^i\right\}}_{i=1}^N$ , constituting an empirical approximation of the filter PDF $p\left(x_t\mathrm{<mo>\; |\; </mo>}{y}_{\mathrm{1\; <mo>\; :\; </mo>}t}\right)$ . Since these weighted particles ${\left\{x_t^i\mathrm{,}{w}_t^i\right\}}_{i=1}^N$ can be used to iteratively approximate the filter PDF at all future times, this completes the PF-algorithm with an overall computational complexity of $O\left(N\right)$ (as pioneered by [31] ; for more technical details, see e.g., [30] and the references therein).

We remark that the pinnacle of the PF-theory is the so-called Rao- Blackwellized (or, marginalized) PF, which is a special kind of factored PF, where the state-space dynamics (17) is split into a linear/Gaussian part and a nonlinear/non-Gaussian part, so that each particle has the optimal linear- Gaussian Kalman filter associated to it (see [32] - [40] ). However, in our case of highly nonlinear and adaptive dynamics, the use of Rao-Blackwellization does not give any advantage, while its mathematics is significantly heavier than in an ordinary PF.

5. Hamiltonian Servo Framework for General Control and Estimation

⁵Half continuous-time, half discrete-time.

The Hamiltonian servo system is our hybrid⁵ global control-estimation framework for a large team/fleet of UGVs (e.g., $10\times 10=100$ ), consisting of the following four components:

Adaptive Control, which is nonlinear and high-dimensional, defined by:

${\stackrel{\dot{}}{q}}_{\alpha \beta }=\phi \left(A^q-q_{\alpha \beta }-{\omega }_{\alpha \beta }^q{q}_{\alpha \beta }^2{p}_{\alpha \beta }\right)-{\displaystyle \underset{j,k}{\sum }}{\partial }_{p_{\alpha \beta }}{H}_{\alpha \beta }{u}_{\alpha \beta }^q,$ (23)

${\stackrel{\dot{}}{p}}_{\alpha \beta }=\phi \left(A^p-p_{\alpha \beta }-{\omega }_{\alpha \beta }^p{p}_{\alpha \beta }^2{q}_{\alpha \beta }\right)+{\displaystyle \underset{j,k}{\sum }}{\partial }_{q_{\alpha \beta }}{H}_{\alpha \beta }{u}_{\alpha \beta }^p,$

${\stackrel{\dot{}}{\omega }}_{\alpha \beta }^q=-{\omega }_{\alpha \beta }^q+{\Phi }_{\alpha \beta }^q\left(q_{\alpha \beta },p_{\alpha \beta }\right),{\stackrel{\dot{}}{\omega }}_{\alpha \beta }^p=-{\omega }_{\alpha \beta }^p+{\Phi }_{\alpha \beta }^p\left(q_{\alpha \beta },p_{\alpha \beta }\right),$ (24)

${\Phi }_{\alpha \beta }^q=S_{\alpha \beta }^q\left(q_{\alpha \beta }\right)S_{\alpha \beta }^q\left(p_{\alpha \beta }\right)+{\stackrel{\dot{}}{S}}_{\alpha \beta }^q\left(q_{\alpha \beta }\right){\stackrel{\dot{}}{S}}_{\alpha \beta }^q\left(p_{\alpha \beta }\right),\left(\alpha ,\beta =1,\cdots ,n\right)$

${\Phi }_{\alpha \beta }^p=S_{\alpha \beta }^p\left(q_{\alpha \beta }\right)S_{\alpha \beta }^p\left(p_{\alpha \beta }\right)+{\stackrel{\dot{}}{S}}_{\alpha \beta }^p\left(q_{\alpha \beta }\right){\stackrel{\dot{}}{S}}_{\alpha \beta }^p\left(p_{\alpha \beta }\right)\mathrm{.}$

Measurement process, given in discrete-time, probabilistic and state-space Hamiltonian ( $q\mathrm{,}p$ )-form:

$p_t^{\alpha \beta }\mathrm{<mo>\; |\; </mo>}{q}_t^{\alpha \beta }~h\left(p_t^{\alpha \beta }\mathrm{<mo>\; |\; </mo>}{q}_t^{\alpha \beta }\right)\iff p_t^{\alpha \beta }=h\left(q_t^{\alpha \beta }\right)+{\nu }_t\mathrm{,}\text{with:}\mathrm{<mi>\; </mi>\; <mi>\; </mi>}{q}_1^{\alpha \beta }~\mu \left(q_1^{\alpha \beta }\right)\mathrm{,}$ (25)

where ${\nu }_t$ is a non-Gaussian measurement noise. Note that here we use the inverse dynamics (given coordinates, calculate velocities, accelerations and forces)―as a bridge between robot’s self-localization and control: measuring coordinates $q_t^{\alpha \beta }$ at discrete time steps t and from them calculating momenta $p_t^{\alpha \beta }$ and control forces ${\stackrel{\dot{}}{p}}_t^{\alpha \beta }$ ;

Bayesian recursions, given in discrete-time Hamiltonian ( $q\mathrm{,}p$ )-form:

$\stackrel{\text{New}\text{Filter}}{p\left(q_t^{\alpha \beta }\mathrm{<mo>\; |\; </mo>}{p}_{\mathrm{1\; <mo>\; :\; </mo>}t}^{\alpha \beta }\right)}=\frac{\stackrel{\text{Measure}}{h\left(p_t^{\alpha \beta }\mathrm{<mo>\; |\; </mo>}{q}_t^{\alpha \beta }\right)}\stackrel{\text{Predictor}}{p\left(q_t^{\alpha \beta }\mathrm{<mo>\; |\; </mo>}{p}_{\mathrm{1\; <mo>\; :\; </mo>}t-1}^{\alpha \beta }\right)}}{p\left(p_t^{\alpha \beta }\mathrm{<mo>\; |\; </mo>}{p}_{\mathrm{1\; <mo>\; :\; </mo>}t-1}^{\alpha \beta }\right)}\mathrm{,}$

where

$\stackrel{\text{Predictor}}{p\left(q_t^{\alpha \beta }\mathrm{<mo>\; |\; </mo>}{p}_{\mathrm{1\; <mo>\; :\; </mo>}t-1}^{\alpha \beta }\right)}={\displaystyle \int }\stackrel{\text{Dynamics}}{f\left(q_t^{\alpha \beta }\mathrm{<mo>\; |\; </mo>}{q}_{t-1}^{\alpha \beta }\right)}\stackrel{\text{Old}\text{Filter}}{p\left(q_{t-1}^{\alpha \beta }\mathrm{<mo>\; |\; </mo>}{p}_{\mathrm{1\; <mo>\; :\; </mo>}t-1}^{\alpha \beta }\right)}\text{d}{x}_{t-1}\mathrm{<mo>\; ;\; </mo>}$ (26)

and

$\stackrel{\text{FilterPDF}}{p\left(q_t^{\alpha \beta }\mathrm{<mo>\; |\; </mo>}{p}_{\mathrm{1\; <mo>\; :\; </mo>}t}^{\alpha \beta }\right)}\approx \frac{\stackrel{\text{Measure}}{h\left(p_t^{\alpha \beta }\mathrm{<mo>\; |\; </mo>}{q}_t^{\alpha \beta }\right)}}{p\left(p_t^{\alpha \beta }\mathrm{<mo>\; |\; </mo>}{p}_{\mathrm{1\; <mo>\; :\; </mo>}t-1}^{\alpha \beta }\right)}{\displaystyle \underset{\left(i\right)=1}{\overset{N}{\sum }}}\stackrel{\text{We}i\text{ghted}\text{Dynamics}}{w_{t-1}^{\left(i\right)}f\left(q_t^{\alpha \beta }\mathrm{<mo>\; |\; </mo>}{q}_{t-1}^{\alpha \beta \left(i\right)}\right)}\mathrm{,}$ (27)

with IS weights:

$w_t^{\left(i\right)}=\frac{h\left(p_t^{\alpha \beta }|q_t^{\alpha \beta \left(i\right)}\right){\displaystyle \underset{n=1}{\overset{N}{\sum }}}{w}_{t-1}^{\left(n\right)}f\left(q_t^{\alpha \beta \left(i\right)}|q_{t-1}^{\alpha \beta \left(n\right)}\right)}{{\displaystyle \underset{n=1}{\overset{N}{\sum }}}{w}_{t-1}^{\left(n\right)}f\left(q_t^{\alpha \beta }|q_{t-1}^{\alpha \beta \left(n\right)}\right)},$

Figure 5. Sample output of the Hamiltonian servo framework applied to a large fleet (a $9\times 9$ matrix) of 81 UGVs. As expected, the plot shows almost periodic time evolution of coordinates $q_{\alpha \beta }\left(t\right)$ and momenta $p_{\alpha \beta }\left(t\right)$ . Slight variation of both coordinates $q_{\alpha \beta }\left(t\right)$ and momenta $p_{\alpha \beta }\left(t\right)$ from the intended regular harmonic motion is due to adaptive control with initial random distribution of synaptic weights ${\omega }_{\alpha \beta }\left(t\right)$ . This plot is the simulation output of the Hamiltonian neural network (23)-(24) and recurrent estimation (larger amplitudes, governed by the Bayesian particle filter (25)-(27)).

where bracketed superscripts $\left(i\mathrm{,}n\right)$ label IS weights.

This Hamiltonian servo framework has been implemented in C++ language (using Visual Studio 2015; see Appendix 1.2). A sample simulation output of the servo system is given in Figure 5. The Figure demonstrates the basic stability and convergence of the servo system.

6. Conclusion

In this paper we have outlined a novel control and estimation framework called the Hamiltonian Servo system. The framework was shown to be a generalization of the Kalman Servo System. Using an example of a large team/fleet of 81 ( $9\times 9$ ) unmanned ground vehicles (UGVs), it was shown that the framework described provided the control and estimation as required. This approach encompassed a three tired system of adaptive control, measurement process and Bayesian recursive filtering which was demonstrated to enable control and estimation of multidimensional, non-linear and non-Gaussian systems.

Acknowledgements

The authors are grateful to Dr. Martin Oxenham, JOAD, DST Group, Australia for his constructive comments which have improved the quality of this paper.

Appendix: C++ Code

1.1. Affine Hamiltonian Neural Network

1.2. High-Dimensional Bayesian Particle Filter

The following C++ code uses the Armadillo matrix library [41] .

NOTES

¹The acronym LQR means linear quadratic regulator.

²The acronym LQG means linear quadratic Gaussian.

³For simplicity, we are dealing with a square $\left(n\times n\right)$ -matrix of UGV configurations. The whole approach works also for a rectangular $\left(n\times m\right)$ -matrix, in which $\alpha =1,\cdots ,n,\mathrm{<mi>\; </mi>}\beta =1,\cdots ,m$ .

Conflicts of Interest

The authors declare no conflicts of interest.

References

[1]	Hui, K.-P. and Pourbeik, P. (2011) OPAL—A Survivability-Oriented Approach to Management of Tactical Military Networks. IEEE MILCOM, 7-10 November 2011.
[2]	Nicholas, P., et al. (2016) Measuring the Operational Impact of Military SATCOM Degradation. Winter Simulation Conference (WSC), 11-14 December 2016.
[3]	Jormakha, J., et al. (2011) UAV-Based Sensor Networks for Future Force Warriors. International Journal of Advances in Telecommunications, 4, 58-71.
[4]	Offensive Swarm-Enabled Tactics (OFFSET). https://www.grants.gov/web/grants/view-opportunity.html?oppId=291825
[5]	Vukobratovic, M. and Potkonjak, V. (1982) Scientific Fundamentals of Robotics, Vol. 1, Dynamics of Manipulation Robots: Theory and Application. Springer, Berlin.
[6]	Vukobratovic, M. and Stokic, D. (1982) Scientific Fundamentals of Robotics, Vol. 2, Control of Manipulation Robots: Theory and Application. Springer, Berlin.
[7]	Vukobratovic, M. and Kircanski, M. (1985) Scientific Fundamentals of Robotics, Vol. 3, Kinematics and Trajectories Synthesis of Manipulation Robots. Springer, Berlin.
[8]	Vukobratovic, M. and Kircanski, N. (1985) Scientific Fundamentals of Robotics, Vol. 4, Real-Time Dynamics of Manipulation Robots. Springer, Berlin.
[9]	Vukobratovic, M., Stokic, D. and Kircanski, N. (1985) Scientific Fundamentals of Robotics, Vol. 5, Non-Adaptive and Adaptive Control of Manipulation Robots. Springer, Berlin.
[10]	Vukobratovic, M. and Potkonjak, V. (1985) Scientific Fundamentals of Robotics, Vol. 6, Applied Dynamics and CAD of Manipulation Robots. Springer, Berlin.
[11]	Ivancevic, V. and Ivancevic, T. (2006) Human-Like Biomechanics. Springer, Dordrecht.
[12]	ASIMO (2016) The Honda Humanoid Robot. http://world.honda.com/ASIMO/
[13]	Ivancevic, V.G. (2010) Nonlinear Complexity of Human Biodynamics Engine, Nonlin. Dynamics, 61, 123-139.
[14]	Durrant-Whyte, H. and Bailey, T. (2006) Simultaneous Localization and Mapping (SLAM): Part I. IEEE Robotics & Automation Magazine, 13, 99-110. https://doi.org/10.1109/MRA.2006.1638022
[15]	Wikipedia (2016) Simultaneous Localization and Mapping.
[16]	Montemerlo, M., Thrun, S., Koller, D. and Wegbreit, B. (2002) FastSLAM: A Factored Solution to the Simultaneous Localization and Mapping Problem.
[17]	Montemerlo, M., Thrun, S., Koller, D. and Wegbreit, B. (2003) FastSLAM 2.0: An Improved Particle Filtering Algorithm for Simultaneous Localization and Mapping That Provably Converges. Proceedings of IJCAI, 1151-1156.
[18]	Thrun, S., Montemerlo, M., Koller, D., Wegbreit, B., Nieto, J. and Nebot, E. (2004) FastSLAM: An Efficient Solution to the Simultaneous Localization and Mapping Problem with Unknown Data Association. Journal of Machine Learning Research, 1-48.
[19]	OpenSLAM (2016). https://www.openslam.org/
[20]	Kalman, R.E. (1960) A New Approach to Linear Filtering and Prediction Problems. Journal of Basic Engineering, 82, 34-45. https://doi.org/10.1115/1.3662552
[21]	Kalman, R.E. and Bucy, R.S. (1961) New Results in Linear Filtering and Prediction Theory. Journal of Basic Engineering, 96, 95-108. https://doi.org/10.1115/1.3658902
[22]	Kalman, R.E., Falb, P. and Arbib, M.A. (1969) Topics in Mathematical System Theory. McGraw Hill, New York.
[23]	Ivancevic, V. and Yue, Y. (2016) Hamiltonian Dynamics and Control of a Joint Autonomous Land-Air Operation. Nonlinear Dynamics, 84, 1853-1865. https://doi.org/10.1007/s11071-016-2610-y
[24]	Ivancevic, V. and Ivancevic, T. (2006) Geometrical Dynamics of Complex Systems. Springer, Dordrecht. https://doi.org/10.1007/1-4020-4545-X
[25]	Ivancevic, V. and Ivancevic, T. (2007) Neuro-Fuzzy Associative Machinery for Comprehensive Brain and Cognition Modelling. Springer, Berlin. https://doi.org/10.1007/978-3-540-48396-0
[26]	Kosko, B. (1992) Neural Networks and Fuzzy Systems, A Dynamical Systems Approach to Machine Intelligence. Prentice-Hall, New York.
[27]	Arulampalam, S., Maskell, S., Gordon, N. and Clapp, T. (2002) A Tutorial on Particle Filters for Online Nonlinear/Non-Gaussian Bayesian Tracking. IEEE Transactions on Signal Processing, 50, 174-188. https://doi.org/10.1109/78.978374
[28]	Ristic, B., Arulampalam, S. and Gordon, N. (2004) Beyond the Kalman Filter. Artech House.
[29]	Schön, T.B., Gustafsson, F. and Nordlund, P.-J. (2005) Marginalized Particle Filters for Mixed Linear/Nonlinear State-Space Models. IEEE Transactions on Signal Processing, 53, 2279-2289. https://doi.org/10.1109/TSP.2005.849151
[30]	Schön, T.B., Lindsten, F., Dahlin, J., et al. (2015) Sequential Monte Carlo Methods for System Identification. IFAC-PapersOnLine, 48, 775-786.
[31]	Gordon, N.J., Salmond, D.J. and Smith, A.F.M. (1993) Novel Approach to Nonlinear/Non-Gaussian Bayesian State Estimation. I Radar and Signal Processing, 140, 107-113.
[32]	Casella, G. and Robert, C.P. (1996) Rao-Blackwellisation of Sampling Schemes. Biometrika, 83, 81-94. https://doi.org/10.1093/biomet/83.1.81
[33]	Chen, R. and Liu, J.S. (2000) Mixture Kalman Filters. Journal of the Royal Statistical Society, 62, 493-508. https://doi.org/10.1111/1467-9868.00246
[34]	Doucet, A., Godsill, S.J. and Andrieu, C. (2000) On Sequential Monte Carlo Sampling Methods for Bayesian Filtering. Statistics and Computing, 10, 197-208. https://doi.org/10.1023/A:1008935410038
[35]	Doucet, A., Gordon, N. and Krishnamurthy, V. (2001) Particle Filters for State Estimation of Jump Markov Linear Systems. IEEE Transactions on Signal Processing, 49, 613-624. https://doi.org/10.1109/78.905890
[36]	Andrieu, C. and Doucet, A. (2002) Particle Filtering for Partially Observed Gaussian State Space Models. Journal of the Royal Statistical Society, 64, 827-836. https://doi.org/10.1111/1467-9868.00363
[37]	Schön, T., Gustafsson, F. and Nordlund, P.-J. (2003) Marginalized Particle Filters for Nonlinear State-Space Models. Tec. Report LiTH-ISY-R-2548, Linköping Univ.
[38]	Schön, T., Gustafsson, F. and Nordlund, P.-J. (2005) Marginalized Particle Filters for Mixed Linear/Nonlinear State-Space Models. IEEE Transactions on Signal Processing, 53, 2279-2289. https://doi.org/10.1109/TSP.2005.849151
[39]	Karlsson, R., Schön, T. and Gustafsson, F. (2005) Complexity Analysis of the Marginalized Particle Filter. IEEE Transactions on Signal Processing, 53, 4408-4411. https://doi.org/10.1109/TSP.2005.857061
[40]	Schön, T., Karlsson, R. and Gustafsson, F. (2006) The Marginalized Particle Filter in Practice. Aerospace Conference, 4-11 March 2006. https://doi.org/10.1109/AERO.2006.1655922
[41]	Wikipedia (2016) Armadillo (C++ Library).