This paper is concerned with the problem of continuous-time nonlinear filtering for stochastic processes on a connected matrix Lie group. The main contribution of this paper is to derive the feedback particle filter (FPF) algorithm for this problem. In its general form, the FPF is shown to provide a coordinate-free description of the filter that automatically satisfies the geometric constraints of the manifold. The particle dynamics are encapsulated in a Stratonovich stochastic differential equation that retains the feedback structure of the original (Euclidean) FPF. The implementation of the filter requires a solution of a Poisson equation on the Lie group, and two numerical algorithms are described for this purpose. As an example, the FPF is applied to the problem of attitude estimation - a nonlinear filtering problem on the Lie group SO(3). The formulae of the filter are described using both the rotation matrix and the quaternion coordinates. Comparisons are also provided between the FPF and some popular algorithms for attitude estimation, namely the multiplicative EKF, the unscented quaternion estimator, the left invariant EKF, and the invariant ensemble Kalman filter. Numerical simulations are presented to illustrate the comparisons.
Feedback particle filters (FPFs) are Monte-Carlo approximations of the solution of the filtering problem in continuous time. The samples or particles evolve according to a feedback control law in order to track the posterior distribution. However, it is known that by itself, the requirement to track the posterior does not lead to a unique algorithm. Given a particle filter, another one can be constructed by applying a time-dependent transformation of the particles that keeps the posterior distribution invariant. Here, we characterize this gauge freedom within the class of FPFs for the linear-G…
For nonlinear affine in the control systems, a Lie algebraic sufficient condition for sampled-data feedback semi-global stabilization is established. We use this result, in order to derive sufficient conditions for sampled-data feedback stabilization for a couple of three-dimensional systems.
This paper presents theory, application, and comparisons of the feedback particle filter (FPF) algorithm for the problem of attitude estimation. The paper builds upon our recent work on the exact FPF solution of the continuous-time nonlinear filtering problem on compact Lie groups. In this paper, the details of the FPF algorithm are presented for the problem of attitude estimation - a nonlinear filtering problem on SO(3). The quaternions are employed for computational purposes. The algorithm requires a numerical solution of the filter gain function, and two methods are applied for this purpose…
This paper is concerned with the problem of continuous-time nonlinear filtering for stochastic processes on a compact and connected matrix Lie group without boundary, e.g. SO(n) and SE(n), in the presence of real-valued observations. This problem is important to numerous applications in attitude estimation, visual tracking and robotic localization. The main contribution of this paper is to derive the feedback particle filter (FPF) algorithm for this problem. In its general form, the FPF provides a coordinate-free description of the filter that furthermore satisfies the geometric constraints of…
This paper proposes an efficient approach for tuning L1 feedback filter of adaptive controller for multi-input multi-output (MIMO) systems. The feedback filter provides performance that trades off fast closed loop dynamics, robustness margin, and control signal range. Thus appropriate tuning of the filter's parameters is crucial to achieve optimal performance. For MIMO systems, the parameters tuning is challenging and requires a multi-objective performance indices to avoid instability. This paper proposes a fuzzy-based L1 feedback filter design tuned with multi-objective particle swarm optimiz…
For general nonlinear autonomous systems, a Lyapunov characterization for the possibility of semi-global asymptotic stabilizability by means of a time-varying sampled-data feedback is established. We exploit this result in order to derive a Lie algebraic sufficient condition for sampled-data feedback semi-global stabilizability of affine in the control nonlinear systems with non-zero drift terms. The corresponding proposition constitutes an extension of the "Artstein-Sontag" theorem on feedback stabilization.
We study the filtering problem over a Lie group that plays an important role in robotics and aerospace applications. We present a new particle filtering algorithm based on stochastic control. In particular, our algorithm is based on a duality between smoothing and optimal control. Leveraging this duality, we reformulate the smoothing problem into an optimal control problem, and by approximately solving it (using, e.g., iLQR) we establish a superior proposal for particle smoothing. Combining it with a suitably designed sliding window mechanism, we obtain a particle filtering algorithm that suff…
In this paper, we shall first derive the admissible control input of the multivariate feedback particle filter (FPF) by minimizing the f-divergence of the posterior conditional density function and the empirical conditional density of the controlled particles. On the contrast, in the original derivation \cite{YMM}, a special f-divergence, Kullback-Leibler (K-L) divergence, is used in the 1-dimensional nonlinear filtering problems. We show that the control input is invariant under the f-divergence class. That is, the control input satisfies exactly the same equations as those obtained by minimi…
The purpose of this paper is to describe the feedback particle filter algorithm for problems where there are a large number ($M$) of non-interacting agents (targets) with a large number ($M$) of non-agent specific observations (measurements) that originate from these agents. In its basic form, the problem is characterized by data association uncertainty whereby the association between the observations and agents must be deduced in addition to the agent state. In this paper, the large-$M$ limit is interpreted as a problem of collective inference. This viewpoint is used to derive the equation fo…
Feedback control protocols can stabilize and enhance the operation of quantum devices, however, unavoidable delays in the feedback loop adversely affect their performance. We introduce a quantum control methodology, combining open-loop control with quantum filtering, which is not constrained by feedback delays. For the problems studied (rapid purification and rapid measurement) we analytically derive lower bounds on the control performance that are comparable with the best corresponding bounds for feedback protocols.
We consider the problem of steering, via output feedback, the state distribution of a discrete-time, linear stochastic system from an initial Gaussian distribution to a terminal Gaussian distribution with prescribed mean and maximum covariance, subject to probabilistic path constraints on the state. The filtered state is obtained via a Kalman filter, and the problem is formulated as a deterministic convex program in terms of the distribution of the filtered state. We observe that, in the presence of constraints on the state covariance, and in contrast to classical Linear Quadratic Gaussian (LQ…
Feedback particle filter (FPF) is a Monte-Carlo (MC) algorithm to approximate the solution of a stochastic filtering problem. In contrast to conventional particle filters, the Bayesian update step in FPF is implemented via a mean-field type feedback control law. The objective for this paper is to situate the development of FPF and related controlled interacting particle system algorithms within the framework of optimal transportation theory. Starting from the simplest setting of the Bayes' update formula, a coupling viewpoint is introduced to construct particle filters. It is shown that the co…
Particle filters have, in recent years, been found to perform well in highly nonlinear problems as well as in estimation of parameters. However, there is still the problem of particle degeneracy in particle filters which has led to the invention of, among others, feedback particle filters. In this paper, we introduce a stochastically perturbed feedback particle filter and show that it is exact. The novelty is in the fact that the innovation process is stochastically perturbed. Resampled sinkhorn particle filter is also introduced. We then compare their performance with that of other filters in…
We derive gain-tuning rules for the positive and negative spatial-feedback loops of a spatially-distributed filter to change the resolution of its spatial band-pass characteristic accordingly to a wavelet zoom, while preserving temporal stability. The filter design is inspired by the canonical spatial feedback structure of the primary visual cortex and is motivated by understanding attentional control of visual resolution. Besides biology, our control-theoretical design strategy is relevant for the development of neuromorphic multiresolution distributed sensors through the feedback interconnec…
Camera motion estimation from observed scene features is an important task in image processing to increase the accuracy of many methods, e.g. optical flow and structure-from-motion. Due to the curved geometry of the state space SE(3) and the non-linear relation to the observed optical flow, many recent filtering approaches use a first-order approximation and assume a Gaussian a posteriori distribution or restrict the state to Euclidean geometry. The physical model is usually also limited to uniform motions. We propose a second-order minimum energy filter with a generalized kinematic model that…
Two Lie algebroids are presented that are linked to the construction of the linearizing output of an affine in the input nonlinear system. The algorithmic construction of the linearizing output proceeds inductively, and each stage has two structures, namely a codimension one foliation defined through an integrable 1-form $ω$ , and a transversal vectorfield $g$ to the foliation. Each integral manifold of the vectorfield $g$ defines an equivalence class of points. Due to transversality, a leaf of the foliation is chosen to represent these equivalence classes. A Lie groupoid is defined with its b…
We combine conditional state density construction with an extension of the Scenario Approach for stochastic Model Predictive Control to nonlinear systems to yield a novel particle-based formulation of stochastic nonlinear output-feedback Model Predictive Control. Conditional densities given noisy measurement data are propagated via the Particle Filter as an approximate implementation of the Bayesian Filter. This enables a particle-based representation of the conditional state density, or information state, which naturally merges with scenario generation from the current system state. This appr…
The design of an invariant tracking control law for the kinematic car driving on a sphere is discussed. Using a Lie group framework a left-invariant description on SO(3) is derived. Basic geometric considerations allow a direct comparison of the model with the usual planar case. Exploiting the Lie group structure an invariant tracking error is defined and a feedback is designed. Finally, one possible design of an invariant asymptotic observer is sketched.
This paper is concerned with a duality-based approach to derive the linear feedback particle filter (FPF). The FPF is a controlled interacting particle system where the control law is designed to provide an exact solution for the nonlinear filtering problem. For the linear Gaussian special case, certain simplifications arise whereby the linear FPF is identical to the square-root form of the ensemble Kalman filter. For this and for the more general nonlinear non-Gaussian case, it has been an open problem to derive/interpret the FPF control law as a solution of an optimal control problem. In thi…
In this paper, we study the irregular output feedback linear quadratic (LQ) control problem, which is a continuous work of previous works for irregular LQ control [33] where the state is assumed to be exactly known priori. Different from the classic output feedback LQ control problem, the cost function must be modified to guarantee the solvability. In the framework of the modified cost function, it is shown that the separation principle holds and the explicitly optimal controller is given in the feedback form of the Kalman filtering. In particular, the feedback gain is calculated by two Riccat…
In this paper, we relate the feedback capacity of parallel additive colored Gaussian noise (ACGN) channels to a variant of the Kalman filter. By doing so, we obtain lower bounds on the feedback capacity of such channels, as well as the corresponding feedback (recursive) coding schemes, which are essentially power allocation policies with feedback, to achieve the bounds. The results are seen to reduce to existing lower bounds in the case of a single ACGN feedback channel, whereas when it comes to parallel additive white Gaussian noise (AWGN) channels with feedback, the recursive coding scheme r…
Probabilistic Data Association-Feedback Particle Filter for Multiple Target Tracking Applications
Coordinated Science Laboratory and the Department of Mechanical Science and Engineering at the University of Illinois at Urbana-Champaign（伊利诺伊大学厄巴纳-香槟分校协同科学实验室与机械科学与工程系）
In this paper, we propose an approach to designing online feedback controllers for input-saturated robotic systems evolving on Lie groups by extending the recently developed Sequential Action Control (SAC). In contrast to existing feedback controllers, our approach poses the nonconvex constrained nonlinear optimization problem as the tracking of a desired negative mode insertion gradient on the configuration space of a Lie group. This results in a closed-form feedback control law even with input saturation and thus is well suited for online application. In extending SAC to Lie groups, the asso…
In this survey, we describe controlled interacting particle systems (CIPS) to approximate the solution of the optimal filtering and the optimal control problems. Part I of the survey is focussed on the feedback particle filter (FPF) algorithm, its derivation based on optimal transportation theory, and its relationship to the ensemble Kalman filter (EnKF) and the conventional sequential importance sampling-resampling (SIR) particle filters. The central numerical problem of FPF -- to approximate the solution of the Poisson equation -- is described together with the main solution approaches. An a…
In this paper, we relate a feedback channel with any finite-order autoregressive moving-average (ARMA) Gaussian noises to a variant of the Kalman filter. In light of this, we obtain relatively explicit lower bounds on the feedback capacity for such colored Gaussian noises, and the bounds are seen to be consistent with various existing results in the literature. Meanwhile, this variant of the Kalman filter also leads to explicit recursive coding schemes with clear structures to achieve the lower bounds. In general, our results provide an alternative perspective while pointing to potentially tig…
For a control system two major issues can be considered: the stabilizability with respect to a given target, and the minimization of an integral functional (while the trajectories reach this target). Here we consider a problem where stabilizability or controllability are investigated together with the further aim of a "cost regulation", namely a state-dependent upper bounding of the functional. This paper is devoted to a crucial step in the program of establishing a chain of equivalences among degree-k stabilizability with regulated cost, asymptotic controllability with regulated cost, and the…
We introduce the Lie algebra of super-operators associated with a quantum filter, specifically emerging from the Stratonovich calculus. In classical filtering, the analogue algebra leads to a geometric theory of nonlinear filtering which leads to well-known results by Brockett and by Mitter characterizing potential models where the curse-of-dimensionality may be avoided, and finite dimensional filters obtained. We discuss the quantum analogue to these results. In particular, we see that, in the case where all outputs are subjected to homodyne measurement, the Lie algebra of super-operators is …
In this paper, we consider stochastic master equations describing the evolutions of quantum systems interacting with electromagnetic fields undergoing continuous-time measurements. In particular, we study feedback control of quantum spin-1/2 systems in the case of unawareness of initial states and in presence of measurement imperfections. We prove that the fidelity between the actual quantum filter and its associated estimated filter converges to one under appropriate assumption on the feedback controller. This shows the asymptotic convergence of such filters. In addition, for spin-J systems, …
We derive symmetry preserving invariant extended Kalman filters (IEKF) on matrix Lie groups. These Kalman filters have an advantage over conventional extended Kalman filters as the error dynamics for such filters are independent of the group configuration which, in turn, provides a uniform estimate of the region of convergence. The proposed IEKF differs from existing techniques in literature on the account that it is derived using minimal tools from differential geometry that simplifies its representation and derivation to a large extent. The filter error dynamics is defined on the Lie algebra…
Parallel combinations of adaptive filters have been effectively used to improve the performance of adaptive algorithms and address well-known trade-offs, such as convergence rate vs. steady-state error. Nevertheless, typical combinations suffer from a convergence stagnation issue due to the fact that the component filters run independently. Solutions to this issue usually involve conditional transfers of coefficients between filters, which although effective, are hard to generalize to combinations with more filters or when there is no clearly faster adaptive filter. In this work, a more natura…
Three Laws of Multivariable Feedback Systems, Extended Spectral Flatness (Extended Wiener Entropy), 'Uncertainty Principles' in Variance Minimization, and Performance Limitations in Minimum Variance Estimation/Filtering
This paper generalizes the results in [30] concerning feedback stabilization of target states for N-level quantum angular momentum systems undergoing quantum non-demolition measurements (QND) in absence of the knowledge about initial states and parameters. Here we consider multiple measurement operators and study the stabilization toward a chosen target subspace which is a common eigenspace of measurement operators. Under the QND conditions, we show that this analysis provides necessary tools to ensure feedback stabilization based on a simplified filter whose state is a N-dimensional vector. A…
Feedback control of quantum mechanical systems must take into account the probabilistic nature of quantum measurement. We formulate quantum feedback control as a problem of stochastic nonlinear control by considering separately a quantum filtering problem and a state feedback control problem for the filter. We explore the use of stochastic Lyapunov techniques for the design of feedback controllers for quantum spin systems and demonstrate the possibility of stabilizing one outcome of a quantum measurement with unit probability.
Quantum mechanical systems exhibit an inherently probabilistic nature upon measurement which excludes in principle the singular direct observability continual case. Quantum theory of time continuous measurements and quantum prediction theory, developed by the author on the basis of an independent-increment model for quantum noise and nondemolition causality principle in the 80's, solves this problem allowing continual quantum predictions and reducing many quantum information problems like problems of quantum feedback control to the classical stochastic ones. Using explicit indirect observation…
Norwegian University of Science and Technology (NTNU)（挪威科技大学（NTNU））
An unscented Kalman filter for matrix Lie groups is proposed where the time propagation of the state is formulated on the Lie algebra. This is done with the kinematic differential equation of the logarithm, where the inverse of the right Jacobian is used. The sigma points can then be expressed as logarithms in vector form, and time propagation of the sigma points and the computation of the mean and the covariance can be done on the Lie algebra. The resulting formulation is to a large extent based on logarithms in vector form, and is therefore closer to the UKF for systems in $\mathbb{R}^n$. Th…
The engineering and control of devices at the quantum-mechanical level--such as those consisting of small numbers of atoms and photons--is a delicate business. The fundamental uncertainty that is inherently present at this scale manifests itself in the unavoidable presence of noise, making this a novel field of application for stochastic estimation and control theory. In this expository paper we demonstrate estimation and feedback control of quantum mechanical systems in what is essentially a noncommutative version of the binomial model that is popular in mathematical finance. The model is ext…
Error Estimates of the Gain Approximation by Hermite-Galerkin Method in Feedback Particle Filter
School of Mathematical Sciences, Beihang University（北京航空航天大学数学科学学院）；Key Laboratory of Mathematics, Informatics and Behavioral Semantics (LMIB), Beihang University（北京航空航天大学数学、信息与行为语义重点实验室）
