2016 American Control Conference (ACC)

Boston Marriott Copley Place

July 6-8, 2016. Boston, MA, USA

Tracking Control and State Estimation of a Mobile Robot based on NMPC

and MHE
Awantha Jayasiri, Sebastien Gros, and George K. I. Mann
Abstract This paper discusses on providing optimizationbased solutions to the state estimation and tracking control
problems arise in mobile robotics. Estimation problem is solved
using Moving Horizon Estimation (MHE) approach and Nonlinear Model Predictive Control (NMPC) is used for solving the
tracking control problem. The proposed approaches are compared against other two common techniques for state estimation
and control. The methods are successfully implemented in
simulation. The real time feasibility of the proposed controller
is discussed.

I. I NTRODUCTION
Path following control arises as the fundamental motion
control problem in mobile robotics and with the addition
of obstacle avoidance control the solution will be robust,
although it generally leads to higher computation complexity [1]. State estimation in mobile robotics is performed
historically by using Extended Kalman Filters (EKF) and
Unscented Kalman Filters (UKF) [2] and PID control is
adopted for tracking control [3], [4]. However, when inequality constraints are present on states and disturbances,
state estimation and control in non-linear systems becomes
more complicated. In such a case, the state estimation and
control problems with constraints can be reformulated as two
seperate optimal control problems where all previous sensor
and motion command information are taken into account.
Consequently, the computation complexity of the problem
increases due to the growth of the problem dimension when
incorporating the complete information and hence generally undesirable for real-time applications. Using a xedsize window to limit the information processing is usually
adopted to limit the computation burden. The window is
propagated when new data arrives and the old data outside
the window is discarded. In the state estimation domain this
approach is termed as Moving Horizon Estimation (MHE)
[5], [6]. Here, an arrival cost is introduced to approximate
the information outside the window. An objective function to
be optimized is dened including the sensor errors, motion
commands and the arrival cost. The method is suited for
online non-linear estimation with limited computing power
[7]. In the control domain the problem is represented as
nding optimal solutions to an objective function including
non-linear system dynamics solving the open loop constrained optimal control problem in each step. Here, the
approach is named as Non-linear Model Predictive Control
(NMPC). The objective function penalises the deviation of
the control inputs and states from a given reference. The rst
control command is applied to the system ans rest is often
discarded [6], [8]. The MHE and NMPC approaches have

978-1-4673-8681-4/$31.00 2016 AACC

1999

been successfully used for aircraft control [9], localisation

of autonomous underwater vehicles (AUVs) [10], formation
control of multiple robots [11] and unmanned ariel vehicles
(UAV) [12], and learning-based mobile robot path tracking
[13].
For fast acting real-time systems such as mobile robots and
complex situations like obstacle avoidance, the control and
state computation have to be performed efciently within the
sample time for safe and accurate movements. It is important
to investigate the feasibility of such optimization schemes for
real-time implementation and this research effort will address
above problem by using the fast NMPC algorithms [14]. In
this paper, we propose solutions to the state estimation and
tracking control problems of a non-holonomic mobile robot
using an optimal control approach. The resulting optimal
control problems are solved by Sequential Quadratic Programming (SQP), by solving the sparse non-linear programming (NLP) problem through simultaneous direct multiple
shooting [7]. The performance of state estimation using MHE
is compared with that of EKF and the control performance of
NMPC is compared with Dynamic Feedback Linearization
(DFL).
The rest of the paper is organized as: section II models
the mobile robot tracking problem, shows the measurement
and the motion models, and discusses the obstacle avoidance.
Section III describes the MHE and NMPC formulations for
optimum state estimation and control. The implementation
of the approach is presented in section IV. The conclusion
and future work are reported in section V.
II. M ODELING THE TRACKING CONTROL
Let us assume the motion of a mobile robot is described by
its position (x, y) and orientation in Cartesian coordinate
plane. Its kinematics equation can be expressed simply as:

cos 0
(1)
x = sin 0 u
0
1
which follows a non-linear difference equation of the form
x = f (x, u)

(2)

where x = [x y ]T are the system states, and u = [u ]T

are the real motion control inputs of the system, with u as
the linear velocity and as the angular velocity, respectively.
A desired reference trajectory is dened by a reference state
vector xr = [xr yr r ]T and the reference control signal
ur = [ur r ]T and follows (1). As depicted in gure 1, the
error states (xe , ye , e ) are selected in a rotated coordinate

A. Measurement model
We used the P3-AT mobile robot (shown in gure 1(a))
provided by Adept MobileRobots for this research work. We
assume the mobile robot is equipped with range and attitude
measuring sensors so that it can read its heading (), the
range to a landmark (R), and angle to the landmark from
its heading direction (), as shown in the gure 1(b). These
measurements are modeled as:
Z = + n
ZR = ||(xl , yl ) (x, y)|| + nR


yl y
1
Z = tan
+ n
xl x
(a) P3-AT mobile robot

&
y

where sensor noises are distributed as n N (0, 2 ), nR

2
2
N (0, R
) and n N (0, 2 ), having 2 , R
and 2 as
the variances of , R and measurements respectively. The
measurement model is written as Z = h + nM , where Z is
the measurement vector, h is the measurement function, and
nM is the measurement noise respectively. The covariance
matrix of nM is written as Q including the variance of each
sensor noise.
(8)
Z = [Z ZR Z ]T

T



yl y

h = () (||(xl , yl ) (x, y)||) tan1

xl x
(9)
2

0
0
2
0
Q = 0 R
(10)
0
0 2

( xr , yr ,r )

r
(xo, yo)

&
ye

&

&
u

xe

(7)

R (x , y )
l l

(x, y)

&
x
(b) Robots environment

B. Motion model

Fig. 1: The mobile robot and its environment

The real motion inputs u and also have associated noises

and they deviate from the commanded control inputs as:
u = u + nu
= + n
frame and presented as [15]:

xe
cos
sin
xe = ye = sin cos
e
0
0

0
xr x
0 yr y (3)
r
1

Consequently, our tracking control problem is converted in

to a regulation problem where a constant reference is used
to track the trajectory. The dynamics of the error states in
(3) can be derived as [15]:
x e = ye u + ur cos e
y e = xe + ur sin e
e = r

A non-linear function for the error states is written as:

x e = g(xe , ue )

where u and are the control commands of the linear and

angular velocity, nu and n are the associated noises. We
model nu N (0, u2 ) and n N (0, 2 ) having u2 and
2 as the variances of u and respectively. Furthermore,
we assume u2 = 1 u2 + 2 2 and 2 = 3 u2 + 4 2 , with
some i 0, i = 1..4 parameters [16]. The motion model
is written as u = u + nC , where u is the motion vector,
u is the commanded control vector, and nC is the noise in
control respectively. The covariance matrix of nC is written
as P including the variances of both control inputs.

(4)

For this error state model, error control signals are dened
as:
 


ur cos e u
u1e
=
ue =
(5)
u2e
r
(6)

2000

(11)

u = [u ]T

(12)

u = [u ]T
 2

u 0
P =
0 2

(13)
(14)

C. System Constraints
When exerting the recommended control actions the physical limitations of the system need to be considered. The input
saturations can be expressed as umin u umax . If there
are any state constraints in the form of path constraints those

also need to be considered. All of these constrains can be

lumped together as an inequality constraint function of the
form:
(u, x) 0
(15)

solving the following dynamic optimization function in each

iteration:
1
t0
HE
= argmin
||uk uk ||2P 1 +
JtM
0
x,u

t0


Also, a non-linear constraint function for error dynamics can

be derived as:
(ue , xe ) 0
(16)
D. Reference Trajectory Generation
Let us assume a desired, feasible and twice differentiable
output trajectory is given as (xr , yr ). Based on (1) following
reference trajectories can be derived using suitable initial
conditions xr(t=0) , yr(t=0) and r(t=0) .
x r = ur cos r
y r = ur sin r

ur = + x 2r + y r2
r y r
yr x r x
r =
u2r
t
r =
r dt

(17)

k=t0 TE +1

where the notation ||e||2V stands for eT V e, matrix A(xt0 TE )

is the arrival cost, xt0 TE represents the state estimation at
time t0 TE , and f is a numerical integration of the system
dynamics over the selected time grid. The term is the
inequality constraint function in (15).
Typically the optimum estimate of the current states is
used and the rest is discarded, though it is used to warmstart the SQP algorithm at the next time instant. The selection
of the arrival cost A(xt0 TE ), in order to approximate the
information discarded by the selected time window, has
several approaches. One simple solution is to employ an EKF
approximation.
A(xt0 TE ) = ||xt0 TE x
t0 TE ||1

t0 TE

E. Obstacle Avoidance
Obstacle avoidance problem is addressed by satisfying a
set of quadratic inequality constraints which are non-convex.
Obstacles are assumed as circular in shape. Let (xo,i , yo,i )
be the position of the ith obstacle and ro,i is its radius. The
following constraints can be achieved [17]:
2
i, ||(xo,i , yo,i ) (x, y)||2 ro,i

(18)

However, having such a tight constraint may not be suitable

for physical implementations as it is safe to keep a minimum
distance from the obstacles. A scaled and relaxed formulation
of (18) can be written as:
||(xo,i , yo,i ) (x, y)||2
Si 0
2 (1 +  )2
ro,i
i

(19)

where Si R+ is a slack variable and i a relative back-off

for ith obstacle.
III. MHE

AND

NMPC

||Zk hk ||2Q1 + A(xt0 TE ) (20)

s.t. xk+1 = f (xk , uk ), (uk , xk ) 0

i, 1

k=t0 TE

(21)

where x
t0 TE is the state estimation for time t0 TE
obtained at the previous MHE run. The matrix sequence
{t } is obtained by repeatedly solving the matrix Riccati
equation as follows.
t+1

Gt t GTt + Vt Pt VtT

1
Gt t HtT Ht t HtT + Qt
Ht t GTt(22)



Kt

where Gt and Vt are Jacobian matrices of (2) with respect

to x and u at time t respectively, and Ht is the jacobian of
the measurement function in (9) with respect to x at t. The
matrices Pt , Qt represent P , Q at t and the Kalman gain at
t is denoted by Kt .

B. System Control based on NMPC

FORMULATIONS

A. State Estimation based on MHE

The real control inputs applied to the system can be
different from the commanded inputs due to the actuator
noises. Also, the real measurements can differ from the
expected ones due to the sensor noise. Hence, the MHE
scheme is formulated based on a non-linear least squares
function (LSQ), penalizing the deviations of the commanded
inputs and the expected measurements from there actual
values and weighted according to their noise covariances.
The computational complexity is limited by considering
only a xed-size information window of length TE and
approximating the prior information in the arrival cost.
The MHE at time t0 computes series of optimum system
0
0 1
and motion commands {uk }tk=t
by
states {xk }tk=t
0 TE
0 TE

2001

An NMPC approach is employed to generate the control

commands for the mobile robot navigation. The formulation
of NMPC scheme is also based on a LSQ, penalizing the
deviations of the inputs and states from their reference. The
NMPC weights are selected according to the importance
attributed to corresponding deviations. A xed-size moving
horizon of length TH is selected to include only a nite set of
future inputs. We use the error dynamics (3)-(6) in the NMPC
scheme, resulting a regulation problem driving the state and
input error to zero. The state dynamics are also discretised
using multiple-shooting. The NMPC solves the following
optimal control problem for each iteration, providing series
0 +TH
0 +TH
of optimal motion commands {uk }tk=t
, states {xk }tk=t
,
0
0
and a set of slack variables Si . The rst control input is
applied to the system and the rest is discarded. The optimal

control problem used in the NMPC scheme reads as:

= argmin

t0 
+TH

xe ,ue ,Si k=t0

s.t. xe,k+1

||ue,k ||2L +

t0 
+TH
k=t0

||xe,k ||2M +

UR

JtN0 M P C

WiT Si
= g(xe,k , ue,k ), (ue,k , xe,k ) 0,
Si 0

S

IV. I MPLEMENTATION
The MHE and NMPC schemes based approach is implemented in simulation. The values of the model parameters
used in the simulation are depicted in table I.
TABLE I: Parameter values used in the simulation
Symbol
dt
R

1 , 2
3 , 4
u

TE
TH
0

Value
0.1
0.01
0.001
0.001
0.01,0.001
0.001,0.01
0.0 u 2.0
2.0 2.0
1.0
1.0
0.1 I3

G G

(23)

where L and M are constant positive denite weighting

matrices and Si is the slacks for obstacle avoidance dened
in (19), and g is a numerical integration of the system error
dynamics over the selected time grid. The term is the
constraint function for error dynamics in (16). A sufciently
high value for Wi guarantees that Si = 0 if avoiding the
obstacle is feasible.

Parameter
Sample time
Noise in range
Noise in heading
Noise in attitude
Noise in motion
Noise in motion
Linear velocity
Angular velocity
MHE horizon
NMPC horizon
Initial state covariance

SR

Unit
s
m
rad
rad
ms1
rads1
s
s
m2 , m2 , rad2

A. Comparison estimator: EKF

The MHE-based localization scheme is compared with the
standard EKF-based localization, using the model parameters
in table I. In both cases NMPC is employed to generate the
velocity commands for simulation.
B. Comparison Controller: DFL
A non-linear controller is implemented for reference tracking based on dynamic feedback linearization as [18]:
1 = x
r + kp1 (xr x) + kd1 (x r x)

2 = yr + kp2 (yr y) + kd2 (y r y)

(24)
u = 1 cos + 2 sin
2 cos 1 sin
=
u
with kp1 , kp1 (= 1.5), and kd1 , kd1 (= 0.7) as proportional
and derivative gains and u = ur at the beginning. The static
collision avoidance is achieved by modifying the trajectory
near the obstacles so that the robot follows the enlarged
perimeter of the obstacle, keeping a safe distance until it
reaches to the next set of way points. This is depicted in
gure 2. While ||po pr || > r0 + 1 + 2 , the robot follows
the pre-dened trajectory. When r0 + 1 < ||po pr || <
r0 + 1 + 2 , the pre-dened trajectory is modied and the

2002

S

SU


Fig. 2:

Trajectory modication for obstacle avoidance

robot is assigned a collision free path from point p1 to p2

as shown. At p2 the robot is again assigned the previous
trajectory.
Simulations are performed for following four cases:
1) State estimation by EKF and control using DFL
2) State estimation by MHE and control using DFL
3) State estimation by MHE and control using NMPC
4) State estimation by EKF and control using NMPC
Figure 3 on the next page shows the navigation results of
these four approaches. The evolution of control signals of
rst three methods are shown in gure 4. Errors in states
and their 3 margins of both EKF and MHE localization
schemes are presented in gure 5.
C. Real-time implementation
In this work, numerical solutions to the problem in (23) is
computed using the code generation option of the ACADO
toolkit [14], based on multiple shooting, sequential quadratic
programming and Real-Time Iteration (RTI) approaches. RTI
provides good approximations for the problem (23) where
only one step for control computation is required. The code
generation option of ACADO provides tailored solutions to
(23) where highly efcient source code is generated, which
is desirable to implement real-time optimal control.
The implementations are performed using a mobile robot
developed by MobileRobots, Inc. in an ofce environment
and the results are shown in gure 6. The robot was
able to follow the given circular trajectory and avoid the
obstacle smoothly. The supported video le shows this
implementation in 4X speed. In all of these implementations the prediction horizon (TH ) is selected as 5s with a
sample time (dt) of 0.25s providing 20 control intervals.
The weighting matrices are selected as: L = diag([10 10])
and M = diag([10 10 0.01]) with W1 = 200000 for
obstacle avoidance. For compute the solutions fourth order
Runge-Kutta integration was selected with online active-set
quadratic programming solver (qpOASES). The code was
implemented on a 2.9GHz/64 bits CPU and the commands
were sent to the mobile robot through a wireless network.
Figure 7 shows the execution times for control computation
in each iteration. The average and the maximum times
required for control computation were 0.87ms and 9.67ms
respectively, which are very much less than the sample time
(0.25s). Increased execution time in gure 7 corresponds to

(a) Navigation in EKF-DFL

(b) Navigation in MHE-DFL

(c) Navigation in MHE-NMPC (d) Navigation in EKF-NMPC

Fig. 3: Navigation results of the four approaches

(a) Evolution of u and in EKF-DFL

(a) Localization error and 3 interval in EKF-NMPC scheme

(b) Evolution of u and in MHE-DFL

u (ms1 )

0.15

0.1

0.05
0

100

200

100

200

300

400

500

600

700

300

400

500

600

700

(rads1 )

1
0.5
0
-0.5
-1

Time ( 0.2s)

(b) Localization error and 3 interval in MHE-NMPC scheme

(c) Evolution of u and in MHE-NMPC

Fig. 4: Evolution of the control signals

Fig. 5: Errors and 3 margins in both localization schemes

2003

(a) Following the circular (b) Avoiding the obstacle (c) Avoiding the obstacle (d) Moving to the prede- (e) Following the circular
trajectory
ned trajectory
trajectory again

Execution time (s)

Fig. 6: Real-time navigation results of the approach using a physical robot

0.01

R EFERENCES

0.008

[1] J. Chunyu, Z. Qu, E. Pollak, and M. Falash, A new reactive target

tracking control with obstacle avoidance in a dynamic environment,
American Control Conference, pp. 38723877, JUN 2009.
[2] S. J. Julier and J. K. Uhlmann, Unscented ltering and nonlinear
estimation, Proceedings of the IEEE, vol. 92, no. 3, pp. 401422,
MAR 2004.
[3] J. E. Normey-Rico, I. Alcal, J. Gmez-Ortega, and E. F. Camacho,
Mobile robot path tracking using a robust PID controller, Control
Engineering Practice, vol. 9, no. 11, pp. 1209 1214, 2001.
[4] P. K. Padhy, T. Sasaki, S. Nakamura, and H. Hashimoto, Modeling
and position control of mobile robot, in Advanced Motion Control,
2010 11th IEEE International Workshop on, March 2010, pp. 100
105.
[5] A. Alessandri, M. Baglietto, and G. Battistelli, Moving-horizon state
estimation for nonlinear discrete-time systems: New stability results
and approximation schemes, Automatica, vol. 44, no. 7, pp. 1753
1765, Mar 2008.
[6] M. Diehl, H. J. Ferreau, and N. Haverbeke, Nonlinear Model Predictive Control. Springer, 2009, ch. Efcient Numerical Methods for
Nonlinear MPC and Moving Horizon Estimation, pp. 391417.
[7] M. Diehl, H. Bock, H. Diedam, and P.-B. Wieber, Fast Motions in
Biomechanics and Robotics. Springer Berlin Heidelberg, 2006, ch.
Fast Direct Multiple Shooting Algorithms for Optimal Robot Control,
pp. 6593.
[8] R. Findeisen and F. Allgwer, An introduction to nonlinear model
predictive, in Control, 21st Benelux Meeting on Systems and Control,
Veidhoven, 2002, pp. 123.
[9] M. Zanon, S. Gros, and M. Diehl, Rotational start-up of tethered
airplanes based on nonlinear mpc and mhe, in Control Conference
(ECC), 2013 European, July 2013, pp. 10231028.
[10] S. Wang, L. Chen, H. Hu, and D. Gu, An optimization based moving
horizon estimation with application to localization of autonomous
underwater vehicles, Robotics and Autonomous Systems, vol. 62,
no. 10, pp. 15811596, Oct 2014.
[11] T. P. Nascimento, A. G. S. Conceicao, and A. P. Moreira, Multi-robot
nonlinear model predictive formation control: the obstacle avoidance
problem, Robotica, vol. FirstView, pp. 119, Sept 2014.
[12] Z. Chao, S.-L. Zhou, L. Ming, and W.-G. Zhang, UAV formation
ight based on nonlinear model predictive control, Mathematical
Problems in Engineering, vol. 2012, pp. 115, 2012.
[13] C. Ostafew, A. Schoellig, and T. Barfoot, Learning-based nonlinear
model predictive control to improve vision-based mobile robot pathtracking in challenging outdoor environments, in Robotics and Automation (ICRA), 2014 IEEE International Conference on, May 2014,
pp. 40294036.
[14] B. Houska, H. J. Ferreau, and M. Diehl, Acado toolkit- an open source
framework for automatic control and dynamic optimization, Optimal
Control Applications and Methods, vol. 32, no. 3, pp. 298312, 2011.
[15] D. Gu and H. Hu, Receding horizon tracking control of wheeled
mobile robots, IEEE Transactions on Control Systems Technology,
vol. 14, no. 4, pp. 743749, JULY 2006.
[16] S. Thrun, W. Burgard, , and D. Fox, Probabilistic Robotics. MIT
Press, 2005.
[17] S. Gros, R. Quirynen, and M. Diehl, Aircraft control based on fast
non-linear mpc & multiple-shooting, in Decision and Control (CDC),
2012 IEEE 51st Annual Conference on, Dec 2012, pp. 11421147.
[18] G. Oriolo, A. De Luca, and M. Vendittelli, WMR control via dynamic
feedback linearization: design, implementation, and experimental validation, Control Systems Technology, IEEE Transactions on, vol. 10,
no. 6, pp. 835852, Nov 2002.

0.006
0.004
0.002
0
0

100

200

300

400

500

Time steps

Fig. 7: Execution times for control computation in real-time

implementation

the situation where the robot was avoiding the obstacle. This
event makes activating and deactivating the constraints a lot,
yielding more changes to the active-set and hence being more
demanding on qpOASES. As an alternative, interior-point
method will give a consistent computational time regardless
of the obstacles.
V. C ONCLUSION &

FUTURE WORK

In this work we have developed an NMPC and MHE-based

system for mobile robot trajectory tracking and non-linear
state estimation. Several simulations are performed with
obstacle avoidance, comparing performance of proposed approach with other existing methods. It is shown that NMPCbased control shows better performance. However, in state
estimation of simple non-linear models the performance of
MHE over other approaches is almost same. A real-time implementation also performed using the ACADO toolkit and a
Pioneer mobile robot. The time required for NMPC control
computation was very much less than the sample time, which
shows the feasibility in real-time control. Another option
is to combine both DFL and NMPC approaches. The fast
DFL approach can be used for no-obstacle environment and
the control law can be switched to smooth NMPC in the
presence of obstacles. This will yield to better results from
both methods.
As the future work, we will explore the feasibility of
implementing the approach in a heterogeneous multi robot
missions including unmanned aerial vehicles and ground
mobile robots. Furthermore, explore the robustness of decentralized and distributed implementations with actuator and
sensor failures will be an interesting research.

2004