Motion planning for autonomous vehicles sharing the road with human drivers remains challenging. The difficulty arises from three challenging aspects: human drivers are 1) multi-modal, 2) interacting with the autonomous vehicle, and 3) actively making decisions based on the current state of the traffic scene. We propose a motion planning framework based on Branch Model Predictive Control to deal with these challenges. The multi-modality is addressed by considering multiple future outcomes associated with different decisions taken by the human driver. The interactive nature of humans is considered by modeling them as reactive agents impacted by the actions of the autonomous vehicle. Finally, we consider a model developed in human neuroscience studies as a possible way of encoding the decision making process of human drivers. We present simulation results in various scenarios, showing the advantages of the proposed method and its ability to plan assertive maneuvers that convey intent to humans.

We consider the problem to learn a step-size policy for the Limited-Memory Broyden-Fletcher-Goldfarb-Shanno (L-BFGS) algorithm. This is a limited computational memory quasi-Newton method widely used for deterministic unconstrained optimization. However, L-BFGS is currently avoided in large-scale problems for requiring step sizes to be provided at each iteration. Current methodologies for the step size selection for L-BFGS use heuristic tuning of design parameters and massive re-evaluations of the objective function and gradient to find appropriate step-lengths. We propose a neural network architecture with local information of the current iterate as the input. The step-length policy is learned from data of similar optimization problems, avoids additional evaluations of the objective function, and guarantees that the output step remains inside a pre-defined interval. The corresponding training procedure is formulated as a stochastic optimization problem using the backpropagation through time algorithm. The performance of the proposed method is evaluated on the training of image classifiers for the MNIST database for handwritten digits and for CIFAR-10. The results show that the proposed algorithm outperforms heuristically tuned optimizers such as ADAM, RMSprop, L-BFGS with a backtracking line search, and L-BFGS with a constant step size. The numerical results also show that a learned policy can be used as a warm-start to train new policies for different problems after a few additional training steps, highlighting its potential use in multiple large-scale optimization problems.

We consider fixed-interval smoothing problems for counter-adversarial autonomous systems. An adversary deploys an autonomous filtering and control system that i) measures our current state via a noisy sensor, ii) computes a posterior estimate (belief) and iii) takes an action that we can observe. Based on such observed actions and our knowledge of our state sequence, we aim to estimate the adversary's past and current beliefs - this forms a foundation for predicting, and counteracting against, future actions. We derive the optimal smoother for the adversary's beliefs (we treat the problem in a Bayesian framework). Moreover, we demonstrate how the smoother can be computed for discrete systems even though the corresponding backward variables do not admit a finite-dimensional characterization. Finally, we illustrate our results in numerical simulations.

Kernel and linear regression have been recently explored in the prediction of graph signals as the output, given arbitrary input signals that are agnostic to the graph. In many real-world problems, the graph expands over time as new nodes get introduced. Keeping this premise in mind, we propose a method to recursively obtain the optimal prediction or regression coefficients for the recently proposed Linear Regression over Graphs (LRG), as the graph expands with incoming nodes. This comes as a natural consequence of the structure of the regression problem, and obviates the need to solve a new regression problem each time a new node is added. Experiments with real-world graph signals show that our approach results in a good prediction performance which tends to be close to that obtained from knowing the entire graph apriori.

Can the parameters of a hidden Markov model (HMM) be estimated from a single sweep through the observations – and additionally, without being trapped at a local optimum in the likelihood surface? That is the premise of recent method of moments algorithms devised for HMMs. In these, correlations between consecutive pair- or triplet-wise observations are empirically estimated and used to compute estimates of the HMM parameters. Albeit computationally very attractive, the main drawback is that by restricting to only low-order correlations in the data, information is being neglected which results in a loss of accuracy (compared to standard maximum likelihood schemes). In this paper, we propose extending these methods (both pair- and triplet-based) by also including non-consecutive correlations in a way which does not significantly increase the computational cost (which scales linearly with the number of additional lags included). We prove strong consistency of the new methods, and demonstrate an improved performance in numerical experiments on both synthetic and real-world financial time-series datasets.

Driving heavy-duty vehicles, such as buses and tractor-trailer vehicles, is a difficult task in comparison to passenger cars. Most research on motion planning for autonomous vehicles has focused on passenger vehicles, and many unique challenges associated with heavy-duty vehicles remain open. However, recent works have started to tackle the particular difficulties related to on-road motion planning for buses and tractor-trailer vehicles using numerical optimization approaches. In this work, we propose a framework to design an optimization objective to be used in motion planners. Based on geometric derivations, the method finds the optimal trade-off between the conflicting objectives of centering different axles of the vehicle in the lane. For the buses, we consider the front and rear axles trade-off, whereas for articulated vehicles, we consider the tractor and trailer rear axles trade-off. Our results show that the proposed design strategy produces planned paths that considerably improve the behavior of heavy-duty vehicles by keeping the whole vehicle body in the center of the lane.

This paper presents a path following controller for autonomous vehicles, making use of the linear time-varying model predictive control (LTV-MPC) framework. The controller takes into consideration control input rates and accelerations, not only to account for limitations in the steering dynamics, but also to provide a safe and comfortable ride while minimizing wear and tear of the vehicle components. Furthermore, it introduces a method to handle model references generated by motion planning algorithms that can consider different vehicle models from the controller. The proposed controller is verified by simulations and through experiments in a Scania construction truck, and is shown to have better performance than the state-of-the-art smooth and accurate MPC.

Lattice-based motion planners are an established method to generate feasible motions for car-like vehicles. However, the solution paths can only reach a discretized approximation of the intended goal pose. Moreover, they can be optimal only with respect to the actions available to the planner, which can result in paths with excessive steering. These drawbacks have a negative impact when used in real systems. In this paper we address both drawbacks by integrating a steering method into a state-of-the-art lattice-based motion planner. Un- like previous approaches, in which path optimization happens in an a posteriori step after the planner has found a solution, we propose an interleaved execution of path planning and path optimization. The proposed approach can run in real-time and is implemented in a full-size autonomous truck, and we show experimentally that it is able to greatly improve the quality of the solutions provided by a lattice planner.

This paper considers a number of related inverse filtering problems for hidden Markov models (HMMs). In particular, given a sequence of state posteriors and the system dynamics; i) estimate the corresponding sequence of observations, ii) estimate the observation likelihoods, and iii) jointly estimate the observation likelihoods and the observation sequence. We show how to avoid a computationally expensive mixed integer linear program (MILP) by exploiting the algebraic structure of the HMM filter using simple linear algebra operations, and provide conditions for when the quantities can be uniquely reconstructed. We also propose a solution to the more general case where the posteriors are noisily observed. Finally, the proposed inverse filtering algorithms are evaluated on real-world polysomnographic data used for automatic sleep segmentation.

We consider estimating the transition probability matrix of a finite-state finite-observation alphabet hidden Markov model with known observation probabilities. We propose a two-step algorithm: a method of moments estimator (formulated as a convex optimization problem) followed by a single iteration of a Newton-Raphson maximum-likelihood estimator. The two-fold contribution of this letter is, first, to theoretically show that the proposed estimator is consistent and asymptotically efficient, and second, to numerically show that the method is computationally less demanding than conventional methods-in particular for large datasets.

In this paper, we propose a controller for smooth autonomous path following. The controller is formulated as an economic model predictive controller. The economic cost introduced in the objective function leads to a smooth driving, since we minimize the first and second derivatives of the curvature function (i.e., we encourage linear curvature profiles). Since the curvature in clothoids varies linearly with the path arc-length, we use the smoothness and comfort characteristics of clothoid-driving to obtain a compact and intuitive controller formulation. We enforce convergence of the controller to the reference path with soft constraints that avoid deviations from the reference path. Finally, we present real life experiments where the controller is deployed on a Scania construction truck that show that the proposed controller outperforms a pure-pursuit controller. Moreover, we detail how the few tuning parameters can affect the obtained solution in practice.

It is often desirable to find the underlying trends in time series data. This is a well known signal processing problem that has many applications in areas such as financial data analysis, climatology, biological and medical sciences. Mean filtering finds a piece-wise constant trend in the data while trend filtering finds a piece-wise linear trend. When the signal is noisy, the main difficulty is finding the changing points in the data that mark the transition points when the mean or the trend changes. Previously proposed methods based on ℓ1filtering suffer from the occurrence of false changing points in the estimate. This is known as the stair-case effect. The main contribution in this paper is incorporating a technique to remove these false changing points to a fast mean filtering algorithm, referred to as the taut-string method, resulting in an efficient procedure with accurate change point detection and thus the removal of the stair-case effect.

In this paper we study the estimation of changing trends in time-series using ℓ1trend filtering. This method generalizes 1D Total Variation (TV) denoising for detection of step changes in means to detecting changes in trends, and it relies on a convex optimization problem for which there are very efficient numerical algorithms. It is known that TV denoising suffers from the so-called stair-case effect, which leads to detecting false change points. The objective of this paper is to show that ℓ1trend filtering also suffers from a certain stair-case problem. The analysis is based on an interpretation of the dual variables of the optimization problem in the method as integrated random walk. We discuss consistency conditions for ℓ1trend filtering, how to monitor their fulfillment, and how to modify the algorithm to avoid the stair-case false detection problem.

We consider a class of weighted nuclear norm optimization problems with important applications in signal processing, system identification, and model order reduction. The nuclear norm is commonly used as a convex heuristic for matrix rank constraints. Our objective is to minimize a quadratic cost subject to a nuclear norm constraint on a linear function of the decision variables, where the trade-off between the fit and the constraint is governed by a regularization parameter. The main contribution is an algorithm to determine the so-called approximate regularization path, which is the optimal solution up to a given error tolerance as a function of the regularization parameter. The advantage is that we only have to solve the optimization problem for a fixed number of values of the regularization parameter, with guaranteed error tolerance. The algorithm is exemplified on a weighted Hankel matrix model order reduction problem.

We address the problem of estimating the number of people in a room using information available in standard HVAC systems. We propose an estimation scheme based on two phases. In the first phase, we assume the availability of pilot data and identify a model for the dynamic relations occurring between occupancy levels, CO2concentration and room temperature. In the second phase, we make use of the identified model to formulate the occupancy estimation task as a deconvolution problem. In particular, we aim at obtaining an estimated occupancy pattern by trading off between adherence to the current measurements and regularity of the pattern. To achieve this goal, we employ a special instance of the so-called fused lasso estimator, which promotes piecewise constant estimates by including an ℓ1norm-dependent term in the associated cost function. We extend the proposed estimator to include different sources of information, such as actuation of the ventilation system and door opening/closing events. We also provide conditions under which the occupancy estimator provides correct estimates within a guaranteed probability. We test the estimator running experiments on a real testbed, in order to compare it with other occupancy estimation techniques and assess the value of having additional information sources. Note to Practitioners - Home automation systems benefit from automatic recognition of human presence in the built environment. Since dedicated hardware is costly, it may be preferable to detect occupancy with software-based systems which do not require the installation of additional devices. The object of this study is the reconstruction of occupancy patterns in a room using measurements of concentration, temperature, fresh air inflow, and door opening/closing events. All these signals are information sources often available in HVAC systems of modern buildings and homes. We assess the value of such information sources in terms of their relevance in detecting occupancy in small and medium-sized rooms. The proposed estimation scheme is composed of two distinct phases. The first is a training phase where the goal is to derive a mathematical model relating the number of occupants with the concentration. It is required to record the actual occupants in the room for a time period spanning few days, a task that can be performed either with manual logging or with temporary dedicated hardware counting systems. In a second phase, we use the derived model to design an online software which collects measurements of the environmental signals and provides the number of people currently in the room. The estimated occupancy levels can then be employed to enhance the efficiency of the HVAC system of the building. We notice that, in modern residential buildings composed by structurally equal flats, the training phase can be run in one flat only, since the obtained model will be reasonably valid for the other flats.

Square-root (in particular, Cholesky) factorization of Toeplitz matrices and of their inverses is a classical area of research. The Schur algorithm yields directly the Cholesky factorization of a symmetric Toeplitz matrix, whereas the Levinson algorithm does the same for the inverse matrix. The objective of this letter is to use results from the theory of rational orthonormal functions to derive square-root factorizations of the inverse of an ntimesn positive definite Toeplitz matrix. The main result is a new factorization based on the Takenaka-Malmquist functions, that is parameterized by the roots of the corresponding auto-regressive polynomial of order n . We will also discuss briefly the connection between our analysis and some classical results such as Schur polynomials and the Gohberg-Semencul inversion formula.

This paper presents a method for grasp evaluation. It is based on the ability of the grasp to reject disturbance forces. The procedure takes the geometry of object into account, and it is also possible to incorporate task-oriented information. The evaluation criterion is formulated as a min-max optimization problem, for which an efficient algorithm is proposed and analyzed. The result of this algorithm is independent of scale and choice of reference frame, and can easily be visualized as a surface in the force space. The method is illustrated with several examples.

This paper treats the problem of approximating a complex stochastic process in a given frequency region by an estimated autoregressive (AR) model. Two frequency domain approaches are discussed: a weighted frequency domain maximum likelihood method and a prefiltered covariance extension method based on the theory of Lindquist and coworkers. It is shown that these two approaches are very closely related and can both be formulated as convex optimization problems. An examples illustrating the methods and the effect of prefiltering/weighting is provided. The results show that these methods are capable of tuning the AR model fit to a specified frequency region.

Consider the hidden Markov model estimation problem where the realization of a single Markov chain is observed by a number of noisy sensors. The sensor scheduling problem for the resulting hidden Markov model is as follows: design an optimal algorithm for selecting at each time instant, one of the many sensors to provide the next measurement. Each measurement has an associated measurement cost. The problem is to select an optimal measurement scheduling policy, so as to minimize a cost function of estimation errors and measurement costs. The problem of determining the optimal measurement policy is solved via stochastic dynamic programming. An optimal finite dimensional algorithm is presented along with numerical results.

A control scheme is proposed for stabilization of backward driving along simple paths for a miniaturized vehicle composed of a truck and a two-axle trailer. The paths chosen are straight lines and arcs of circles. When reversing, the truck and trailer under examination can be modeled as an unstable nonlinear system with state and input saturations. The simplified goal of stabilizing along a trajectory (instead of a point) allows us to consider a system with controllable linearization. Still, the combination of instability and saturations makes the task impossible with a single controller. In fact, the system cannot be driven backward from all initial states because of the jack-knife effects between the parts of the multibody vehicle; it is sometimes necessary to drive forward to enter into a specific region of attraction. This leads to the use of hybrid controllers. The scheme has been implemented and successfully used to reverse the radio-controlled vehicle.

The problem of detection and discrimination of double talk and change in the echo path in a telephone channel is considered. The phenomenon echo path change requires fast adaptation of the channel model to be able to equalize the echo dynamics. On the other hand, the adaption rate should be reduced when double talk occurs. Thus, it is critical to quickly detect a change in the echo path while not confusing it with double talk, which gives a similar effect. The proposed likelihood based approach compares a global channel model with a local one over a sliding window, both estimated with the recursive least squares algorithm.

This paper presents a new method to detect and discriminate between abrupt changes in dynamics and sudden changes in disturbance levels in dynamic systems. It is assumed that a normalized least mean square (NLMS) adaptive filter estimates the system. The detection method is based on the observation that the estimated taps behave differently in the two studied events. The convergence behavior of the taps is modeled using averaging theory, giving an exponential convergence behavior for each tap. Kalman filtering techniques, based on this model, are then used in order to design a new detection scheme.

This paper presents a novel approach to blind equalization (deconvolution), which is based on direct examination of possible input sequences. In contrast to many other approaches, it does not rely on a model of the approximative inverse of the channel dynamics. To start with, the blind equalization identifiability problem for a noise-free finite impulse response channel model is investigated. A necessary condition for the input, which is algorithm independent, for blind deconvolution is derived. This condition is expressed in an information measure of the input sequence. A sufficient condition for identifiability is also inferred, which imposes a constraint on the true channel dynamics. The analysis motivates a recursive algorithm where all permissible input sequences are examined. The exact solution is guaranteed to be found as soon as it is possible. An upper bound on the computational complexity of the algorithm is given. This algorithm is then generalized to cope with time-varying infinite impulse response channel models with additive noise. The estimated sequence is an arbitrary good approximation of the maximum a posteriori estimate. The proposed method is evaluated on a Rayleigh fading communication channel. The simulation results indicate fast convergence properties and good tracking abilities.>

The authors' approach to blind equalization examines the possible input sequences directly by using a bank of filters and, in contrast to common approaches, does not try to find an approximative inverse of the channel dynamics. The identifiability question of a noise-free finite impulse response (FIR) model is investigated. A sufficient condition for the input sequence (persistently exciting of a certain order) is given which guarantees that both the channel model and the input sequence can be determined exactly in finite time. A recursive algorithm is given for a time-varying infinite impulse response (IIR) channel model with additive noise, which does not require a training sequence. The estimated sequence is an arbitrarily good approximation of the maximum a posteriori estimate. The proposed method is evaluated on a Rayleigh fading communication channel. It shows fast convergence properties and good tracking ability.>

Many practical applications of signal processing require accurate determination of signal parameters from sensor array measurements. Most estimation techniques are sensitive to errors in the array response model. Thus, reliable array calibration schemes are of great importance. A paradigm for generating an array model from noise corrupted calibration vectors is developed. The key idea is to use a local parametric model of the sensor responses. The potential improvement using the suggested scheme is demonstrated on real data collected from a full-scale hydroacoustic array.>

The application of adaptive antenna techniques to increase the channel capacity in mobile radio communication is discussed. Directional sensitivity is obtained by using an antenna array at the base station, possibly both in receiving and transmitting mode. A scheme for separating several signals at the same frequency is proposed. The method is based on high-resolution direction finding following by optimal combination of the antenna outputs. Comparisons to a method based on reference signals are made. Computer simulations are carried out to test the applicability of the technique to scattering scenarios that typically arise in urban areas. The proposed scheme is found to have great potential in rejecting cochannel interference, albeit at the expense of high computational requirements.>

Signal parameter estimators which are less sensitive to perturbations in the array manifold are presented. A parametrized stochastic model for the array uncertainties is introduced. The unknown array parameters can include the individual gain and phase responses of the sensors as well as their positions. Based on this model, a maximum a posteriori (MAP) estimator is formulated. This results in a fairly complex optimization problem which is computationally expensive. The MAP estimator is simplified by exploiting properties of the weighted subspace fitting method. An approximate method that further reduces the complexity is also presented, assuming small array perturbations. A compact expression for the MAP Cramer-Rao bound (CRB) on the signal and array parameter estimates is derived. A simulation study indicates that the proposed robust estimation procedures achieve the MAP-CRB even for moderate sample sizes.>