Pitfalls of Constraint Kalman Filtering

Yingjie Hu; Demoz Gebre-Egziabher

doi:10.33012/navi.771

Abstract

In this work, the impact of state constraints in Kalman filters is analyzed. Such constraints are commonly used in integrated navigation systems to augment sensor measurements, for example, through zero-velocity updates in personal navigation systems. Constraints may be linear or nonlinear and may be posed as either equality or inequality constraints. It is shown that linear inequality constraints can introduce bias into the state estimate that is not reflected in the estimation error covariance matrix. Furthermore, for nonlinear equality and inequality constraints, projection-based methods that have been used in the design of integrated navigation systems can yield inconsistent estimates, in which the computed state estimation error covariance matrix is a poor proxy for the actual estimation error statistics.

Keywords

1 INTRODUCTION

Information from external aiding sensors is an integral component of inertial sensor-based positioning, navigation, and timing (PNT) systems. While global navigation satellite systems, optical sensors, radars, and other similar sensors have been used in this capacity, leveraging constraints provide another source of aiding information that is sometimes overlooked. Constraints describe known relations between state variables, which can arise from the kinematics (Skog et al., 2010), dynamics (Keyes et al., 2023; Koifman & Bar-Itzhack, 1999; Yu et al., 2018), geometric characteristics of the system (Hu et al., 2022; Skog et al., 2012), performance specifications (Hu et al., 2024; Simon & Simon, 2010), etc. By incorporating constraints into the filter, it is possible to arrest or mitigate drift in the navigation state vector estimates. The constraint Kalman filter typically follows two methodologies: the pseudo-observation approach and the projection approach. The pseudo-observation approach handles equality constraints by treating them as observations/measurements. The projection approach can deal with both equality and inequality constraints, by casting a constrained optimization problem as follows:

$\begin{array}{l} {\hat{x}}_{k}^{*} = \arg \min_{\tilde{x}} {(\tilde{x} - {\hat{x}}_{k}^{+})}^{T} {P_{k}}^{+ - 1} (\tilde{x} - {\hat{x}}_{k}^{+}) & s.t. & C (\tilde{x}) = 0 & or & C (\tilde{x}) \leq 0 \end{array}$ 1

where $\tilde{x}$ denotes the optimization variable, ${\hat{x}}_{k}^{+}$ is the unconstrained state estimate, and $P_{k}^{+}$ is the covariance matrix. $C (\tilde{x})$ represents the constraint. ${\hat{x}}_{k}^{*}$ is the solution to the constrained optimization problem, which can be expressed as ${\hat{x}}_{k}^{*} = p r o j ({\hat{x}}_{k}^{+})$ . $p r o j (\cdot)$ denotes the projection operator, which projects the unconstrained estimate ${\hat{x}}_{k}^{+}$ onto the constraint surface, yielding ${\hat{x}}_{k}^{*}$ .

Prior work has examined the projection approach formulated in Equation (1). Simon and Chia (2002) derived a closed-form solution of the projection function $p r o j (\cdot)$ for linear equality constraints and presented an analysis of the constrained state and covariance matrix, providing valuable insights into how states and covariance matrices are influenced by linear equality constraints. However, this analysis does not apply to nonlinear or inequality constraints. Regarding nonlinear constraints, Simon and Chia (2002) applied an linearization approach to convert a nonlinear constraint into a linear form. However, linearization errors are introduced in this process, and the constrained estimate is not guaranteed to satisfy the original nonlinear constraint.

An analytical solution was proposed by Yang and Blasch (2009) to compute the constrained state estimate when the nonlinear constraint is in a quadratic form. However, this solution does not address the associated covariance matrix of the constrained estimate. Skog et al. (2012) applied the analytical solution derived by Yang and Blasch (2009) to a pedestrian navigation problem in which the inter-foot distance of a human body is treated as an inequality constraint. In these applications, the covariance matrix of the constrained estimate is computed using Equation (8) of Julier and LaViola (2007). However, none of those works, including the original paper (Julier & LaViola, 2007) on which they were based, presented a derivation of this constrained covariance matrix equation. The purpose of this engineering note is to analyze the effect of using nonlinear and inequality constraints in a Kalman filter, thereby filling this gap in the existing literature and bringing attention to caveats and pitfalls that have been overlooked when using such constraints in Kalman filters.

This work contributes to the field by showing that the state estimates obtained by using constraints in Kalman filters can be biased. Furthermore, the resulting covariance matrix may not be a representative or accurate bound of the estimation error statistics. Accordingly, the remainder of this paper is organized as follows. In Section 2, we present a state and covariance matrix analysis of the projection approach considering linear/nonlinear and equality/inequality constraints. In Section 3, we highlight the caveats and pitfalls of using these constraints in a Kalman filter. Finally, Section 4 concludes the paper.

2 STATE AND COVARIANCE ANALYSIS

2.1 Linear Equality Constraint

The algorithm for a linear equality constraint was derived by Simon and Chia (2002) and is briefly discussed here for completeness. Perhaps one of the most well-known linear equality constraints in PNT is the zero-velocity update used in personal navigation (Skog et al., 2010). Given a linear equality constraint $D x = d$ , Equation (1) is written as follows:

${\hat{x}}^{*} = \arg min_{\tilde{x}} {(\tilde{x} - {\hat{x}}^{+})}^{T} P^{+^{- 1}} (\tilde{x} - {\hat{x}}^{+}) s.t. D \tilde{x} = d$ 2

where $D \in ℝ^{m \times n}, d \in ℝ^{m \times 1}$ , and the subscript k is dropped for brevity. The constrained state estimate ${\hat{x}}_{k}^{*}$ can be expressed as follows:

$\begin{matrix} {\hat{x}}^{*} & = & {\hat{x}}^{+} - P^{+} D^{T} {(D P^{+} D^{T})}^{- 1} (D {\hat{x}}^{+} - d) = F {\hat{x}}^{+} + b \\ \begin{matrix} where & F \end{matrix} & = & \begin{matrix} I - P^{+} D^{T} {(D P^{+} D^{T})}^{- 1} D, & b = P^{+} D^{T} {(D P^{+} D^{T})}^{- 1} d \end{matrix} \end{matrix}$ 3

Equation (3) shows that linear equality constraints yield a linear projection. The constrained estimate ${\hat{x}}^{*}$ is unbiased, $E [{\hat{x}}^{*}] = x$ , provided $E [{\hat{x}}^{+}] = x$ , where $E (\cdot)$ denotes the expectation. In addition, the constrained covariance matrix can be computed as follows (Simon & Chia, 2002):

$P^{*} = E [(x - {\hat{x}}^{*}) {(x - {\hat{x}}^{*})}^{T}] = F P^{+} F^{T}$ 4

In addition to being unbiased, Simon and Chia (2002) showed that for linear equality constraints, $P^{+} > P^{*}$ in the positive-definite sense, yielding a consistent estimator with reduced uncertainty. Unlike linear equality constraints, the remaining constraints discussed below do not, in general, possess the desirable properties of unbiasedness and covariance consistency.

2.2 Linear Inequality Constraint

While less common in PNT, linear inequality constraints do arise, for example, in altitude constraints where the estimated altitude $\hat{h}$ must exceed a specified minimum altitude h₀ (Zhu et al., 2005). Simon and Chia (2002) discussed the improvement in estimation quality provided by linear inequality constraints. Here, we instead focus on the bias and covariance consistency properties of linear inequality constraints and their impact on the state estimate and covariance under a projection approach. For a linear inequality constraint, the projection formulation is given by the following:

${\hat{x}}^{*} = \arg min_{\tilde{x}} {(\tilde{x} - {\hat{x}}^{+})}^{T} P^{+^{- 1}} (\tilde{x} - {\hat{x}}^{+}) s.t. D \tilde{x} \leq d$ 5

where $D \in R^{m \times n}$ and $d \in R^{m \times 1}$ . This vector constraint $D \tilde{x} \leq d$ consists of a sequence of scalar constraints $D_{(i, :)} \tilde{x} \leq d_{i}, i = 1, \dots, m$ , in which $D_{(i, :)}$ denotes the i-th row of matrix D and d_i is the i-th entry of vector d. Constraints for which the equality holds at the solution ${\hat{x}}^{*}$ are referred to as active constraints, e.g., $D_{(i, :)} {\hat{x}}^{*} = d_{i}$ . Assuming that the correct set of active constraints is known a priori, they can be assembled to form the equality constraint $\overset{―}{D} {\hat{x}}^{*} = \overset{―}{d}$ , where $\overset{―}{D}$ and $\overset{―}{d}$ are a subset of the entries in D and d. It can be shown that the original problem in Equation (5) is equivalent to the following optimization with linear equality constraints:

${\hat{x}}^{*} = \arg min_{\tilde{x}} {(\tilde{x} - {\hat{x}}^{+})}^{T} P^{+^{- 1}} (\tilde{x} - {\hat{x}}^{+}) s.t. \overset{―}{D} \tilde{x} = \overset{―}{d}$ 6

Thus, the optimal solution ${\hat{x}}^{*}$ can be obtained by using the same approach presented in Section 2.1:

$\begin{matrix} {\hat{x}}^{*} & = & {\hat{x}}^{+} - P^{+} {\bar{D}}^{T} {(\bar{D} P^{+} {\bar{D}}^{T})}^{- 1} (\bar{D} {\hat{x}}^{+} - \bar{d}) = F {\hat{x}}^{+} + b \\ \begin{matrix} where & F \end{matrix} & = & I - P^{+} {\bar{D}}^{T} {(\bar{D} P^{+} {\bar{D}}^{T})}^{- 1} \bar{D}, b = P^{+} {\bar{D}}^{T} {(\bar{D} P^{+} {\bar{D}}^{T})}^{- 1} \bar{d} \end{matrix}$ 7

Although Equation (7) takes the same form as Equation (3), we cannot make the same claim about the unbiasedness and constrained uncertainty as in Section 2.1.

Remark 1: There are no guarantees that the constrained estimate ${\hat{x}}^{*}$ will be unbiased.

Proof: It is a given that the constrained estimate ${\hat{x}}^{*}$ satisfies the active equality constraint $\overset{―}{D} {\hat{x}}^{*} = \overset{―}{d}$ . However, the true state x is constrained by the original inequality constraint $D x \leq d$ in Equation (5). Because $\overset{―}{D}$ and $\overset{―}{d}$ are subsets of D and d, the following holds: $\overset{―}{D} x \leq \overset{―}{d}$ for the true state. We denote the difference between $\overset{―}{D} x$ and $\overset{―}{d}$ by $Δ \overset{―}{d}$ :

$\overset{―}{D} x - \overset{―}{d} - Δ \overset{―}{d} = 0 and Δ \overset{―}{d} \leq 0$ 8

Let us assume that the unconstrained estimate is unbiased, i.e., $E [{\hat{x}}^{+}] = x$ . However, as we will show next, this is not the case for the constrained estimates (Equation (7)). For this, we proceed as follows:

$\begin{matrix} E [{\hat{x}}^{*}] & = & E [{\hat{x}}^{+} - P^{+} {\overset{―}{D}}^{T} {(\overset{―}{D} P^{+} {\overset{―}{D}}^{T})}^{- 1} (\overset{―}{D} {\hat{x}}^{+} - \overset{―}{d})] \\ = & E [{\hat{x}}^{+} - P^{+} {\overset{―}{D}}^{T} {(\overset{―}{D} P^{+} {\overset{―}{D}}^{T})}^{- 1} (\overset{―}{D} {\hat{x}}^{+} - \overset{―}{d} - (\overset{―}{D} x - \overset{―}{d} - Δ \overset{―}{d}))] \\ = & x - P^{+} {\overset{―}{D}}^{T} {(\overset{―}{D} P^{+} {\overset{―}{D}}^{T})}^{- 1} Δ \overset{―}{d} \end{matrix}$ 9

Equation (9) shows that the expectation of the constrained estimate ${\hat{x}}^{*}$ is equal to the true state x plus an additional bias term $P^{+} {\overset{―}{D}}^{T} {(\overset{―}{D} P^{+} {\overset{―}{D}}^{T})}^{- 1} Δ \overset{―}{d}$ . For ${\hat{x}}^{*}$ to be unbiased, either $Δ \overset{―}{d} = 0$ or $Δ \overset{―}{d}$ must lie in the null space of $P^{+} {\overset{―}{D}}^{T} {(\overset{―}{D} P^{+} {\overset{―}{D}}^{T})}^{- 1}$ . In the former case, the inequality constraint $\overset{―}{D} x \leq \overset{―}{d}$ reduces to an equality constraint $\overset{―}{D} x = \overset{―}{d}$ . However, there is no assurance that the latter condition will always be satisfied; thus, no guarantees on the unbiasedness of the estimate can be made.

Remark 2: Let P^∗ denote the constrained covariance matrix obtained by solving the optimization problem defined in Equation (6). In general, there is no guarantee that this covariance estimate is consistent, that is, representative of the true estimation error statistics.

Proof: Using Equations (7) and (8), we can show that $x - {\hat{x}}^{*}$ and $x - {\hat{x}}^{+}$ are related as follows:

$\begin{matrix} x - {\hat{x}}^{*} & = & x - ({\hat{x}}^{+} - P^{+} {\overset{―}{D}}^{T} {(\overset{―}{D} P^{+} {\overset{―}{D}}^{T})}^{- 1} (\overset{―}{D} {\hat{x}}^{+} - \overset{―}{d})) \\ = & x - {\hat{x}}^{+} + P^{+} {\overset{―}{D}}^{T} {(\overset{―}{D} P^{+} {\overset{―}{D}}^{T})}^{- 1} (\overset{―}{D} {\hat{x}}^{+} - \overset{―}{d} - \overset{―}{D} x + \overset{―}{d} + Δ \overset{―}{d}) \\ = & F (x - {\hat{x}}^{+}) + Δ p where Δ p = P^{+} {\overset{―}{D}}^{T} {(\overset{―}{D} P^{+} {\overset{―}{D}}^{T})}^{- 1} Δ \overset{―}{d} \end{matrix}$ 10

The mean squared error (MSE) represents the expected squared difference between the state estimate and the true underlying state. The MSE can be written as follows:

$\begin{matrix} MSE & = E [(x - {\hat{x}}^{*}) {(x - {\hat{x}}^{*})}^{T}] = E [(F (x - {\hat{x}}^{+}) + Δ p) {(F (x - {\hat{x}}^{+}) + Δ p)}^{T}] \\ = F P^{+} F^{T} + Δ p Δ p^{T} \end{matrix}$ 11

However, when the optimal constrained state estimate is denoted by Equation (7), the constrained covariance matrix will be computed as $P^{*} = {F P}^{+} F^{T}$ , which corresponds to the first term in Equation (11). Consequently, MSE $P^{*} = Δ p Δ p^{T} \geq 0$ , which demonstrates that the MSE (characterizing the actual estimation error uncertainty) is greater than or equal to the computed covariance matrix P^∗ in the positive-semidefinite sense. Equality holds when $Δ p$ is the zero vector, implying that $Δ \overset{―}{d} = 0$ and $\overset{―}{D} x = \overset{―}{d}$ .

This result can be understood intuitively as follows. Under the equality constraint, the true state x is strictly confined to the hyperplane defined by $\overset{―}{D} x = \overset{―}{d}$ . However, when this equality constraint is relaxed into an inequality, the feasible region for x expands to the entire volume enclosed by the hyperplane. This expansion introduces additional uncertainty into the constrained state estimate that is not captured by P^∗.

The important takeaway is this: there are no guarantees that the constrained state estimate is unbiased or that the covariance estimate is consistent and reflective of the actual estimation error distribution.

2.3 Nonlinear Equality Constraint

Although often handled in an ad hoc manner, a common nonlinear equality constraint in inertial navigation and attitude determination is the unit-norm quaternion constraint $q^{T} q = 1$ (Titterton & Weston, 1997). Such a nonlinear equality constraint can be systematically formulated using a projection approach as follows:

${\hat{x}}^{*} = arg min_{\tilde{x}} {(\tilde{x} - {\hat{x}}^{+})}^{T} P^{+^{- 1}} (\tilde{x} - {\hat{x}}^{+}) s.t. D (\tilde{x}) = d$ 12

Without specifying the functional form of the nonlinear equality constraint $D (x) = d$ , nothing specific can be said about the constrained state estimate or its covariance. However, some general observation can be made. To that end, without loss of generality, we denote the constrained state estimate by ${\hat{x}}^{*} = p r o j ({\hat{x}}^{+})$ . In the constrained Kalman filtering literature, the following equation has been used to compute the constrained covariance matrix (Julier & LaViola, 2007; Skog et al., 2012):

$P^{*} = \nabla p P^{+} \nabla p^{T}$ 13

where $\nabla p$ is the Jacobian of the projection function $p r o j (\cdot)$ about the unconstrained state estimate. As we will show next, Equation (13) can be a poor approximation to the real state estimation error uncertainty of the constrained state estimate.

Remark 3:Equation (13) can be a poor approximation to the true estimation error statistics of the constrained state estimate ${\hat{x}}^{*}$ even if the errors introduced by the linearization of the projection function $p r o j (\cdot)$ are small and, thus, ignored.

Proof: The projection function $p r o j (\cdot) : R^{n \times 1} \to R^{n \times 1}$ is a nonlinear mapping from the unconstrained state domain ${\hat{x}}^{+}$ to the constrained state domain ${\hat{x}}^{*}$ . Let ${\overset{―}{x}}^{+}$ denote the linearization point, which corresponds to $E [{\hat{x}}^{+}]$ . We approximate the projection function using a first-order Taylor series expansion, neglecting higher-order terms:

${\hat{x}}^{*} = proj ({\hat{x}}^{+}) \approx proj ({\overset{―}{x}}^{+}) + \nabla p \cdot ({\hat{x}}^{+} - {\overset{―}{x}}^{+}) = \nabla p \cdot {\hat{x}}^{+} + Δ x^{+}$ 14

where $Δ x^{+} = p r o j ({\overset{―}{x}}^{+}) - \nabla p \cdot {\overset{―}{x}}^{+}$ . Ignoring the higher-order terms is a tacit assumption that linearization errors are small. Therefore, the MSE of the constrained state estimate can be determined by the following:

$\begin{matrix} MSE & = & E [(x - {\hat{x}}^{*}) {(x - {\hat{x}}^{*})}^{T}] \\ = & E [{\nabla p (x - {\hat{x}}^{+}) + (I - \nabla p) x - Δ x^{+}} {\nabla p (x - {\hat{x}}^{+}) + (I - \nabla p) x - Δ x^{+}}^{T}] \\ = & \nabla p P^{+} \nabla p^{T} + E [\nabla p (x - {\hat{x}}^{+}) {[(I - \nabla p) x - Δ x^{+}]}^{T}] \\ + E [[(I - \nabla p) x - Δ x^{+}] {(x - {\hat{x}}^{+})}^{T} \nabla p^{T}] \\ + E [[(I - \nabla p) x - Δ x^{+}] {[(I - \nabla p) x - Δ x^{+}]}^{T}] \end{matrix}$ 15

Equation (15) contains three additional terms compared with Equation (13), and there is no mathematical guarantee that these three terms are zero without the inclusion of specific assumptions. Therefore, Equation (13) is only an approximation of the true estimation error statistics, even if we disregard the linearization error. Neglecting the last three additional terms in Equation (15) could compromise the consistency between the constrained estimate and its statistical estimation uncertainty. In safety-critical navigation, it is important to conservatively bound the uncertainty of the state errors. Equation (13) could potentially underestimate the estimation error uncertainty, which can undermine the integrity of the navigation system. By inspecting Equation (15), it can be seen that the last three terms drop out when $(I - \nabla p) x - Δ x^{+}$ is equal to zero. It is straightforward to show that this quantity will always be zero for a linear equality constraint. For the general nonlinear equality constraint $D (x) = d$ , there is no mathematical guarantee that the condition $(I - \nabla p) x - Δ x^{+} = 0$ will always hold. Therefore, computing the constrained covariance matrix using Equation (13) is inadequate and can sometimes be erroneous. As a matter of fact, it can lead to numerical singularities, e.g., rank-deficient covariance matrices being computed as mentioned by Julier and LaViola (2007) and Skog et al. (2010). In summary, integrating nonlinear equality constraints into a Kalman filter solution via the projection approach results in a state estimate for which no assurances about unbiasedness can be made. Furthermore, using Equation (13) can result in the generation of a covariance matrix that is not positive definite.

2.4 Nonlinear Inequality Constraint

An example of a nonlinear inequality constraint that appears in PNT applications is the maximum feet separation distance constraint in pedestrian navigation (Skog et al., 2012). This constraint has the form ${‖ x_{1} - x_{2} ‖}_{2} \leq γ^{2}$ , where $x_{1}$ and $x_{2}$ are the position vectors of each foot and γ is the maximum separation distance (step size) between the feet. In the case of a nonlinear inequality constraint, the problem formulation of the projection approach can be expressed as follows:

$\begin{matrix} {\hat{x}}^{*} = \arg \min_{\tilde{x}} {(\tilde{x} - {\hat{x}}^{+})}^{T} P^{+^{- 1}} (\tilde{x} - {\hat{x}}^{+}) & s.t. & D (\tilde{x}) \leq d \end{matrix}$ 16

Following an approach similar to that of Section 2.2, the solution to Equation (16) with an inequality constraint $D (\tilde{x}) \leq d$ is equivalent to the following optimization with an active equality constraint $\overset{―}{D} (\tilde{x}) = \overset{―}{d}$ :

$\begin{matrix} {\hat{x}}^{*} = \arg \min_{\tilde{x}} {(\tilde{x} - {\hat{x}}^{+})}^{T} P^{+^{- 1}} (\tilde{x} - {\hat{x}}^{+}) & s.t. & \bar{D} (\tilde{x}) = \bar{d} \end{matrix}$ 17

where $\overset{―}{D} (\tilde{x}) = \overset{―}{d}$ represents the set of active scalar constraints from $\overset{―}{D} (\tilde{x}) \leq \overset{―}{d}$ .

We can gain the following insight about the constrained covariance matrix $P^{*}$ for a nonlinear inequality constraint:

Remark 3 in Section 2.3 also applies to the nonlinear inequality constraint case here, as the proof in Section 2.3 is based on a general projection function $p r o j (\cdot)$ without assuming that the nonlinear constraint is based on an equality or inequality.

3 NUMERICAL SIMULATION

In this section, numerical simulation results are presented to show the effects of constraints in Kalman filtering. A one-dimensional simplistic example is considered in which two agents are moving along an axis. The system's state vector can be expressed as $x = {[\begin{array}{ll} x_{1} & x_{2} \end{array}]}^{T}$ , with $x_{i}$ being the position of agent i relative to the origin and i = 1,2. Here, we assume that the state estimate ${\hat{x}}^{+}$ is unbiased and has a Gaussian distribution.

Case 1: In this case study, we compare linear inequality constraints with linear equality constraints to demonstrate remark 1 and remark 2. Let us assume that the value of agent 2's position is smaller than that of agent 1 and that the separation cannot exceed 1 . This can be formulated as a linear inequality constraint as $x_{1} - x_{2} \leq 1$ . In this simulation case, the true state x is set to ${[\begin{array}{ll} 2 & 1.2 \end{array}]}^{T}$ . Thus, the unbiased mean is $E [\begin{array}{ll} {\hat{x}}^{+} \end{array}] = {[\begin{array}{ll} 2 & 1.2 \end{array}]}^{T}$ . The covariance matrix $P^{+}$ is set to an identity matrix. A total of 10,000 samples of the unconstrained state estimate ${\hat{x}}^{+}$ are generated from its known distribution, and Equation (7) is used to determine the constrained state estimate ${\hat{x}}^{*}$ . Figure 1(a) presents the samples of the unconstrained state estimate ${\hat{x}}^{+}$ and constrained state estimate ${\hat{x}}^{*}$ . The yellow circle corresponds to the true state x, and the black cross corresponds to $E [{\hat{x}}^{+}]$ . The green asterisk indicates the mean of the constrained state estimate $E [{\hat{x}}^{*}] = {[\begin{array}{ll} 1.764 & 1.439 \end{array}]}^{T} \neq x$ . Therefore, the constrained state estimate ${\hat{x}}^{*}$ is biased, which aligns with Remark 1. Additionally, the MSE can be computed as ${MSE}^{*} = [\begin{matrix} 0.7525 & 0.2433; & 0.2433 & 0.7549 \end{matrix}]$ using Equation (11) along with the simulated true state x and the constrained state estimate samples ${\hat{x}}^{*}$ . However, the constrained covariance matrix is computed as $P^{*} = F P^{+} F^{T} = [\begin{array}{llll} 0.5 & 0.5; & 0.5 & 0.5 \end{array}]$ (matrix F defined in Equation (7)). Therefore, it can be shown that MSE* $\geq P^{*}$ . This aligns with the statement in Remark 2 that the constrained covariance matrix P^∗ is not guaranteed to be a true representative of the actual state estimation error statistics (characterized by the MSE). Additionally, we examine the case in which the constraint becomes a linear equality $x_{1} - x_{2} = 1$ (in this case, $E [{\hat{x}}^{+}] = x = {[\begin{array}{ll} 2 & 1 \end{array}]}^{T}$ ). Figure 1(b) shows the samples of ${\hat{x}}^{+}$ and ${\hat{x}}^{*}$ . The MSE can be computed as $MSE * = [\begin{array}{llll} 0.4940 & 0.4940; & 0.4940 & 0.4940 \end{array}]$ using the simulated true state x and the constrained state estimate samples ${\hat{x}}^{*}$ . The constrained covariance matrix is computed as $P^{*} = F P^{+} F^{T} = [\begin{array}{llll} 0.5 & 0.5; & 0.5 & 0.5 \end{array}]$ (matrix F defined in Equation (3)). This result shows that the computed MSE is consistent with the constrained covariance matrix P^∗ for the linear equality constraint case. This comparison highlights one key takeaway: unlike the linear equality constraint case, a linear inequality constraint can produce a constrained covariance matrix that is not guaranteed to be representative of the true estimation error uncertainty, as shown by the discrepancy between the empirical MSE and covariance matrix P^∗.
Case 2: In this case study, a nonlinear constraint is considered. Let us assume that the distance between the two agents remains 0.8. This nonlinear equality constraint can be formulated as $‖ x_{1} - x_{2} ‖ = 0.8$ . The same simulation setup applied in case 1 is utilized here. The true state x is set to ${[\begin{array}{ll} 2 & 1.2 \end{array}]}^{T}$ . Thus, the mean $E [{\hat{x}}^{+}] = {[\begin{array}{ll} 2 & 1.2 \end{array}]}^{T}$ . The covariance matrix $P^{+}$ is set to an identity matrix. A total of 5,000 samples of the unconstrained state estimate ${\hat{x}}^{+}$ are generated from its known distribution. Then, the constrained state estimate ${\hat{x}}^{*}$ is determined by solving the optimization problem given by Equations (12) and (13). Figure 2 presents the samples of the unconstrained state estimate ${\hat{x}}^{+}$ and constrained state estimate ${\hat{x}}^{*}$ . The yellow circle corresponds to the true state x, and the black cross corresponds to $E [{\hat{x}}^{+}]$ . The green asterisk indicates the mean of the constrained state estimate $E [{\hat{x}}^{*}] = {[\begin{array}{ll} 1.7523 & 1.4160 \end{array}]}^{T} \neq x$ . Therefore, the constrained state estimate ${\hat{x}}^{*}$ is biased. Additionally, the MSE can be computed as ${MSE}^{*} = [\begin{array}{llll} 0.7144 & 0.3160; & 0.3160 & 0.6595 \end{array}]$ by using the constrained samples and the true state x. However, the constrained covariance matrix computed using Equation (13) is $P^{*} = [\begin{array}{llll} 0.5 & 0.5; & 0.5 & 0.5 \end{array}]$ . Compared with ${MSE}^{*}, P^{*}$ underestimates the uncertainty in $x_{1}$ and overestimates the uncertainty in $x_{2}$ , resulting in inconsistency between the actual state estimate uncertainty of ${\hat{x}}^{*}$ and the constrained covariance matrix $P^{*}$ . The result of this simulation case study validates Remark 3.

Samples of the unconstrained state estimate x^+ (blue circles) and constrained state estimate x^∗ (magenta plus signs) for (a) a linear inequality constraint x1−x2≤1 and (b) a linear equality constraint x1−x2=1

FIGURE 1

Samples of the unconstrained state estimate ${\hat{x}}^{+}$ (blue circles) and constrained state estimate ${\hat{x}}^{*}$ (magenta plus signs) for (a) a linear inequality constraint $x_{1} - x_{2} \leq 1$ and (b) a linear equality constraint $x_{1} - x_{2} = 1$

Samples of the unconstrained state estimate x^+(blue circles) and constrained state estimate x^∗ (magenta plus signs) for the constraint ‖x1−x2‖=0.8

FIGURE 2

Samples of the unconstrained state estimate ${\hat{x}}^{+}$ (blue circles) and constrained state estimate ${\hat{x}}^{*}$ (magenta plus signs) for the constraint $‖ x_{1} - x_{2} ‖ = 0.8$

4 SUMMARY

In this paper, we explored the effects of using constraints in Kalman filters. We showed that for inequality constraints, no guarantees about the unbiasedness of the state estimate can be made. Additionally, while inequality constraints offer more flexibility, they can introduce greater uncertainty into the system, resulting in a constrained covariance matrix that does not represent the true state estimation error statistics. For nonlinear constraints, our analysis reveals that an existing method for calculating the constrained covariance matrix P^∗ is flawed in that it can generate improper covariance matrices. The key takeaway from the analysis presented here is that the use of inequality and nonlinear constraints within the framework of Kalman filtering must be approached with care, as the introduction of constraints can lead to biased and potentially inconsistent state estimates.

HOW TO CITE THIS ARTICLE:

Hu, Y., & Gebre-Egziabher, D. (2026). Pitfalls of constraint Kalman filtering. NAVIGATION, 73. https://doi.org/10.33012/navi.771

This is an open access article under the terms of the Creative Commons Attribution License, which permits use, distribution and reproduction in any medium, provided the original work is properly cited.

REFERENCES

↵
Hu, W., Hu, Y., Stas, M., & Farrell, J. A. (2024). Optimization-based outlier accommodation for tightly coupled RTK-aided inertial navigation systems in urban environments. arXiv. https://arxiv.org/abs/2407.13912
Google Scholar
↵
Hu, Y., Ketema, Y., McGovern, R., Jean, J., Jonason, A., Gebre-Egziabher, D., & Hanson, J. (2022, January). Networked inertial navigation with constraints generated by neural networks. In Proceedings of the 2022 International Technical Meeting of the Institute of Navigation (pp. 1405-1419). https://doi.org/10.33012/2022.18187
Google Scholar
↵
Julier, S. J., & LaViola, J. J. (2007). On Kalman filtering with nonlinear equality constraints. IEEE Transactions on Signal Processing, 55(6), 2774-2784. https://doi.org/10.1109/TSP.2007.893949
Google Scholar OpenURL
↵
Keyes, S. A., Wilkens, D. R., & Enns, D. F. (2023). Systems and methods for model based vehicle navigation (US Patent 11,790,793). U.S. Patent and Trademark Office.
Google Scholar
↵
Koifman, M., & Bar-Itzhack, I. Y. (1999). Inertial navigation system aided by aircraft dynamics. IEEE Transactions on Control Systems Technology, 7(4), 487-493. https://doi.org/10.1109/87.772164
Google Scholar OpenURL
↵
Simon, D., & Chia, T. L. (2002). Kalman filtering with state equality constraints. IEEE Transactions on Aerospace and Electronic Systems, 38(1), 128-136. https://doi.org/10.1109/7.993234
Google Scholar OpenURL CrossRef
↵
Simon, D., & Simon, D. L. (2010). Constrained Kalman filtering via density function truncation for turbofan engine health estimation. International Journal of Systems Science, 41(2), 159-171. https://doi.org/10.1080/00207720903042970
Google Scholar OpenURL
↵
Skog, I., Handel, P., Nilsson, J.-O., & Rantakokko, J. (2010). Zero-velocity detection-An algorithm evaluation. IEEE Transactions on Biomedical Engineering, 57(11), 2657-2666. https://doi.org/10.1109/TBME.2010.2060723
Google Scholar OpenURL CrossRef
↵
Skog, I., Nilsson, J.-O., Zachariah, D., & Händel, P. (2012, November). Fusing the information from two navigation systems using an upper bound on their maximum spatial separation. In International Conference on Indoor Positioning and Indoor Navigation (IPIN) (pp. 1-5). https://doi.org/10.1109/IPIN.2012.6418862
Google Scholar
↵
Titterton, D. H., & Weston, J. L. (1997). Strapdown inertial navigation technology. Peter Peregrinus Ltd.
Google Scholar
↵
Yang, C., & Blasch, E. (2009). Kalman filtering with nonlinear state constraints. IEEE Transactions on Aerospace and Electronic Systems, 45(1), 70-84. https://doi.org/10.1109/TAES.2009.4805264
Google Scholar OpenURL
↵
Yu, Z., Hu, Y., & Huang, J. (2018). GPS/INS/Odometer/DR integrated navigation system aided with vehicular dynamic characteristics for autonomous vehicle application. IFAC-PapersOnLine, 936-942. https://doi.org/10.1016/j.ifacol.2018.10.060
Google Scholar
↵
Zhu, J., Santerre, R., & Chang, X.-W. (2005). A Bayesian method for linear, inequality-constrained adjustment and its application to GPS positioning. Journal of Geodesy, 78(9), 528-534. https://doi.org/10.1007/s00190-004-0425-y
Google Scholar OpenURL

PDF

Previous Next

Pitfalls of Constraint Kalman Filtering

Abstract

1 INTRODUCTION