Kalman Filtering with Equality and Inequality State Constraints

1 Introduction

• State constraints can arise naturally in tracking, yet are often neglected because they can be difficult to model.The introduction gives fixed robotic-arm speed as an equality constraint and maximum motor speed as an inequality constraint.Source: §1 · p. 3
• For equality constraints, the paper projects the updated estimate onto the constrained region or restricts the Kalman Gain so the update satisfies the constraint.The gain-restriction method is shown to be a special case of projection.Source: §1 · p. 3
• Both approaches are extended to inequality constraints, and the paper also addresses constraints on state predictions.The introduction notes prior discussion of aspects of these extensions in and.Source: §1 · p. 3
• Nonlinear constraints can be handled by local linearization or a scheme such as an Unscented Kalman Filter, with accuracy depending on the nonlinear function’s structure and curvature.The experiments apply inequality constraints to tracking with nonlinear dynamics.Source: §1 · p. 3

2 Kalman Filter

• Prediction and updated error covariances represent uncertainty in the state estimates, and the initial covariance must be large enough to contain the initial state.The paper notes that covariance information indicates how much to trust predictions and estimates.Source: §2 · pp. 4–5
• The filter predicts the state forward using the system dynamics, then updates its estimate using measurements available through the current time step.The state prediction uses measurements only through the preceding time step.Source: §2 · p. 4
• The measurement residual is the difference between the observed and predicted measurement, and the Kalman Gain weights this residual in the state update.The gain is selected to minimize mean-square state-estimate error.Source: §2 · p. 5

3 Equality Constrained Kalman Filtering

• The equality-constrained framework can also constrain the state prediction, which the paper says would allow a better forecast.The equality constraints are expressed using a matrix A and vector b acting on the state.Source: §3 · p. 6

3.1 Projecting the state to lie in the constrained space

• The projection method finds a constrained estimate by minimizing its weighted distance from the unconstrained updated estimate.The weighting matrix may be any positive definite symmetric matrix; the resulting estimate has the smallest updated error covariance.Source: §3.1 · pp. 6–7

3.2 Restricting the optimal Kalman Gain so the updated state estimate lies in the constrained space

• Restricting the Kalman Gain forces the updated estimate into the constrained space; its result is equivalent to projection with the identity weighting matrix.The paper derives the constrained gain through a constrained optimization problem.Source: §3.2 · pp. 8, 11

4 Adding Inequality Constraints

• Inequality constraints are enforced on updated estimates through either projection or a restricted Kalman Gain, while constraining state predictions can also improve forecasting.The two update methods parallel those used for equality constraints; the section also notes the option of constraining predictions.Source: §4 · p. 11
• Both inequality-constrained update methods require numerical methods rather than the analytic solution available previously.Source: §4 · p. 11

4.1 By Projecting the Unconstrained Estimate

• The projection method solves an inequality-constrained minimization using an active-set method, treating selected constraints as equalities and ignoring inactive constraints.The active set is updated using nonzero Lagrange multipliers, and iterations continue until the solution meets a tolerance or iteration limit.Source: §4.1 · p. 12
• For the projected estimate's covariance, the projection uses equality constraints only because projecting onto the inequality-constrained subspace is not trivial.A safety term can represent convergence error and should typically be small relative to the unconstrained error covariance.Source: §4.1 · p. 13

4.2 By Restricting the Optimal Kalman Gain

• The alternative method optimizes a Kalman Gain that forces the updated estimate into the constrained space, using an inequality-constrained solver such as SDPT3.Its covariance uses the restricted gain and is then projected using equality constraints only.Source: §4.2 · p. 13

5 Dealing with Nonlinearities

• For nonlinear dynamics and measurement models, the Extended Kalman Filter linearizes each function at the current state prediction or estimate and then proceeds as in the linear filter.The transition Jacobian is evaluated at the previous updated estimate, and the measurement Jacobian at the current prediction.Source: §5 · p. 14

5.1 Nonlinear Equality and Inequality Constraints

• For nonlinear constraints, the constraint function is linearized at the current state prediction, producing an approximate linear constraint usable by the methods already described.The Jacobian is evaluated at the prediction; equality constraints follow by replacing inequality signs with equality signs.Source: §5.1 · p. 14
• Nonlinearities in the models or constraints can also be handled with derivative-free methods, higher-order Taylor approximations, or an iterative method such as the Iterated Extended Kalman Filter.Source: §5.1 · p. 14

6 Constraining the State Prediction

• Constraining the state prediction can improve forecasting, but increases computational cost per iteration.Source: §6 · p. 15
• The first method projects the transition matrix onto the constrained space, a procedure available for equality constraints but with no trivial inequality-space projection.If the updated estimate is constrained, the resulting transition matrix keeps predictions in the equality-constrained space.Source: §6 · p. 15
• A second method solves for a transition matrix when the updated estimate is not constrained, and can incorporate inequality constraints.The covariance matrix can also be constrained.Source: §6 · p. 15
• A third method adds constraints ensuring the chosen estimate produces a constrained prediction at the next iteration.If constraints depend on the estimate, an iterative look-ahead can recalculate the current estimate, but may be far more expensive computationally.Source: §6 · p. 15

7.1 First Experiment

• The first experiment tracks a noisy sine curve with inequality constraints limiting its sine component to [−1,1].The measurements add noise that significantly masks the underlying curve.Source: §7 · p. 16, §7.1 · p. 16, Figure 1 · p. 17
• The sine-curve experiment generates the path with a nonlinear stochastic process and adds noise in the frequency domain while keeping the process recursive.Its transition function uses the sine-curve structure, and the measurement model considerably masks the underlying data.Source: §7 · p. 16, §7.1 · p. 16

7.1 First Experiment; 7.2 Second Experiment

• Both inequality-constrained methods keep estimates within the specified space, while the unconstrained filter can violate the bounds.The second experiment also reports visual improvement near the edges of the constrained space.Source: §7.1 · p. 18, Figure 2 · p. 19, §7.2 · p. 21

7.2 Second Experiment

• The second experiment uses an AR(6) model to estimate an unknown signal, assuming only that its values lie in [−1,1].The constraints apply to the current estimate and all lags; the filter also estimates lags at each iteration using additional information, a fixed-interval smoothing feature.Source: §7.2 · pp. 18, 21
• The AR(6) experiment represents the state with 13 elements: the current estimate, five lags, six coefficients, and a constant.Its process-noise covariance uses 0.1 for the first entry and 10−6 for the others.Source: §7.2 · pp. 20–21

8 Conclusions

• The two examples show estimates remaining in the constrained space, which the authors say provides a better estimate structure.The conclusion does not report a quantitative comparison.Source: §8 · p. 21
• For equality constraints, both formulations have analytic formulas, and one is a special case of the other.The conclusion also describes two numerical methods for inequality-constrained estimates.Source: §8 · p. 21
• The paper also discusses constraining state predictions and handling nonlinearities.These are additional topics beyond constraining the estimate.Source: §8 · p. 21

Appendix A Kron and Vec

• The appendix introduces the right Kronecker product and gives identities for appropriately sized matrices.It also presents an identity combining the vec operator with the Kronecker product.Source: Appendix A · pp. 21–23
• Vectorization maps a matrix to a long vector by stacking its columns iteratively.For an [m×n] matrix, the resulting vector has size [mn×1].Source: Appendix A · p. 23
• The appendix gives a trace expression for a product of matrices and clarifies that a1,n denotes the entry in row 1, column n.Source: Appendix A · pp. 21, 23

Appendix B Analytic Block Representation for the inverse of a Saddle Point Matrix

• The appendix identifies a saddle point matrix by its block form.The displayed block form is introduced but not reproduced in the passage.Source: Appendix B · p. 23
• A subscript S distinguishes these matrices from matrices defined earlier.Source: Appendix B · p. 23
• When AS is nonsingular and the Schur complement condition is met, the inverse of the saddle point matrix can be expressed analytically.The appendix cites for this expression.Source: Appendix B · p. 24

• The appendix formulates the task as solving Mn = p for vector n, with M arranged as a saddle-point matrix to fit Equation (B.1).Source: Appendix C · p. 24
• It gives expressions for JS, S, and the upper-right block of M^-1; a footnote uses Equation (A.2) with B′ S to obtain the same Bs term as Equation (C.1).Source: Appendix C · pp. 24–25
• Because p's first block is zero, the derivation solves for n to obtain l, then states the Lagrange-multiplier vector λ.Source: Appendix C · p. 25