Error-State Kalman Filtering with Linearized State Constraints

Abstract

In recent years, the error-state Kalman filter (ErKF) has been widely employed in various applications, including robotics, aerospace, and localization. However, incorporating state constraints into the ErKF framework using the estimate projection method remains ambiguous. This paper examines this issue in depth, specifically exploring whether constraints should be enforced before or after the ErKF correction step. We adopt a mathematical approach, deriving analytical solutions and analyzing their statistical properties. Our findings prove that, for a linear system with linear constraints, both methods yield statistically equivalent results. However, the filter’s behavior becomes uncertain when dealing with linearized constraints. We further identify a special case of a nonlinear constraint where the results of the linear case remain valid. To support our theoretical analysis and evaluate the filter’s performance under non-ideal conditions, we conduct two Monte Carlo simulations considering increasing initialization errors and constraint incompleteness. The simulation results validate our theoretical insights and suggest that applying constraints to the error state after the correction step may lead to superior performance compared to the alternative approach.

1. Introduction

The incredible Kalman Filter (KF) has become a ubiquitous mathematical tool for linear state estimation problems in a diverse range of fields, including but not limited to robotics [1], aerospace [2], navigation [3], and industrial applications [4]. It provides a globally optimal estimate based on a mathematical model of the system dynamics and a series of noisy sensor measurements [5]. The filter’s ability to fuse information from multiple sensors and its ability to handle time-varying systems make it a popular choice in many fields. Although linear systems are often used in theoretical studies, nonlinear systems are more commonly encountered in practical applications. While many techniques are available for state estimation in nonlinear systems [6], linearization of nonlinearity has emerged as the most popular choice due to its simplicity and effectiveness. The utilization of the linearized system within the framework of the KF is commonly known as the extended Kalman filter (EKF).

Another variant of the KF that is especially popular in attitude estimation and localization problems is the error-state Kalman filter (ErKF). This indirect (error-state) technique is designed to estimate the difference between the actual state vector and the estimated state vector. One of the primary benefits of utilizing the ErKF is the ability to avoid the platform’s dynamic model complexity [7]. Moreover, for a nonlinear estimation problem that involves linearization, it estimates not only the estimation error but also the linearization error. This allows us to obtain a more accurate and reliable estimation [8]. Note that the ErKF and the KF are mathematically equivalent for linear systems. However, the former shows superior numerical stability compared to the latter. For nonlinear systems, although the ErKF and the EKF share mathematical similarity, they are different in the statistical sense [8], and the ErKF provides better numerical performance compared to the EKF.

In many practical applications, certain aspects of system dynamics may be difficult or complex to express mathematically. As a result, researchers often either disregard this information entirely or treat it as an additional noise-free measurement. An alternative approach is to incorporate such information as constraints in the estimation process. When applied within the Kalman filter (KF) framework, this technique is known as the constrained Kalman filter (CKF). By enforcing constraints on the KF’s estimation, CKF methods can account for such information without compromising the accuracy and robustness of the standard KF [9]. In this paper, we focus on scenarios where the constraint is either inherently linear or linearized, given the partial equivalence in their treatment. Consequently, the results derived for linear constraints may be directly applicable to their linearized counterparts [10,11].

As described earlier, the ErKF estimates the error state, not the full state; hence, an additional correction step is required at the end of each algorithm’s epoch to compute the final solution of interest. This extra step introduces ambiguity when applying existing CKF methods to the error state, particularly when using the estimate projection method (EPM). The reason is that the ErKF’s estimate will be reset to zero after the correction ([12], Section 13.6). Consequently, physically speaking, the corrected full state is now carrying the estimation error, not the error state. As a result, the mean and variance of the error state will be altered [13], which potentially affects the constrained estimation. This raises the question of whether the error projection should be applied before or after the correction step. The ambiguity associated with the EPM does not arise in other constraint-based approaches discussed in [9]. Therefore, in this paper, we attempt to comprehensively investigate potential ways for incorporating linear constraints into the ErKF framework through the estimate projection perspective. Additionally, we also aim to determine the circumstances under which these approaches are equivalent or yield distinctive outcomes. Notably, this inquiry has yet to be explored in the existing literature, as far as the authors know. To recap, our key contributions to this article can be summarized as follows:

• We claim two possible approaches with a rigorous mathematical derivation of their respective solutions. Specifically, one method involves placing the constraint before the correction step, while the other involves enforcing the constraint after the correction step.
• Building upon our proposed solution, we examine their statistical properties by leveraging previous results on the full-state CKF in [10]. Particularly, we mathematically prove that, in the linear constraint case, imposing constraints either before or after the correction step yields identical performance in terms of mean square error. Furthermore, we provide the specific conditions under which they hold equivalence when linearized constraints are taken into account.
• We present two numerical examples to evaluate the filter’s performance. In the first example, complete and incomplete constraints are imposed, whereas in the second example, a nonlinear constraint that does not meet our assumption is applied, thus hindering a rigorous assessment of the filter’s behavior. Moreover, we conduct extensive Monte Carlo simulations to scrutinize the filter’s accuracy and robustness across different scenarios of initialization errors. This error factor plays a crucial role in filtering theory [14,15]. The simulation results lend support to our theoretical framework and vision.

The rest of this article is organized as follows: Section 2 reviews related works in the context of constrained filtering. Section 3 provides the mathematical notations, along with a brief overview of both the constrained Kalman filter and the error-state Kalman filter. After laying the foundation, we develop the proposed constrained error-state Kalman filter in Section 4. Next, Section 5 presents two numerical examples with Monte Carlo simulations to validate the filter’s performance. Finally, Section 6 provides concluding remarks and future work directions.

2. Related Works

A great deal of effort and thorough analysis have been developed for the full-state CKF with both linear [10,16] and nonlinear constraints [11,17]. There are various ways to formulate the CKF for linear constraints, including but not limited to the estimate projection method [10], the system projection method (SPM) [18], and the gain projection method (GPM) [19]. While the EPM projects the KF’s estimate onto the constraint surface, the SPM and the GPM project the system dynamics and the Kalman gain onto the constraint surface, respectively. To avoid being overwhelmed by the vast number of approaches available, Simon [9] provides a comprehensive summary of most existing methods with rigorous analysis and comparison from both theoretical and practical implementation perspectives. In contrast to the special treatment and extensive research given to the CKF, the lack of attention given to the constrained ErKF (C-ErKF) in the research community has made it difficult for the author to seek reference.

While there have been some studies of the C-ErKF, its full potential and limitations are not yet fully understood. For instance, Jung and Park [20] incorporated depth constraints into an ErKF-based filter to estimate the poses of a visual–inertial navigation system. Although this filter showed great performance compared to the traditional ErKF, only the optimization problem was given. Without an analytical solution, it is unclear whether the constraint was performed before or after the error correction. This ambiguous expression was also used in [21,22], whereby the error correction was not taken into account during the filter’s design. Note that both of the mentioned researchers used the EPM to yield their solutions. This could potentially be a contributing factor to the occurrence of the unclear problem. In particular, Zanetti et al. [22], Zanetti and Bishop [23] constrained the error state via the GPM. By doing so, the potential impact of error correction may be disregarded, thereby making their approach appear distinct or unparalleled. This observation is applicable to the SPM [9] as well, in which the error covariance and the process noise covariance matrices are subject to certain constraints instead of the KF’s estimate. Whilst one might ignore the confusion and directly apply the CKF’s results to the error-state form in one way or another, we believe that thorough inquiry must be taken into consideration to improve the process of filter selection.

3. Preliminaries

3.1. Mathematical Notation

Throughout the following sections, unless otherwise stated, we shall express ${\mathbb{R}}^n$, ${\mathbb{R}}^{m\times n}$ as the set of n-dimensional Euclidean space and the set of $m\times n$ real matrices, respectively. Scalars and vectors are denoted by lowercase letters x, matrices by capital letters X, and their transposes by $X^{\top }$. The notations $I_n$ and $1_n$ indicate an identity matrix whose dimension is $n\times n$ and an $n\times 1$ all-ones vector, respectively. An $m\times n$ zeros matrix is denoted by $0_{m\times n}$. We shall adopt the symbol $X\circ Y$ to denote the Hadamard product of two matrices X and Y with the same dimensions. A general multivariate Gaussian distribution of a random vector x with mean $\mu$ and covariance matrix P is denoted as $x\sim \mathcal{N}\left(\mu ,P\right)$. Finally, if we say that $X\ge Y$ for any square matrices $X,Y\in {\mathbb{R}}^{n\times n}$, we mean that $X-Y$ is positive semi-definite.

3.2. Constrained Kalman Filter with Linearized Constraints

In this section, we shall proceed to review the problem of linear state estimation that is subjected to a set of known, not necessarily time-invariant, equality constraints. Given these conditions, the topic of finding an optimal solution from the unconstrained estimate of the linear KF has been thoroughly studied and generalized in the remarkable survey [9]. For a brief summary, let us consider a linear time-varying discrete-time dynamical system as follows:

$$\begin{array}{cc}\hfill x_k& =F_{k-1}{x}_{k-1}+w_{k-1}\hfill \end{array}$$

(1a)

$$\begin{array}{cc}\hfill y_k& =H_k{x}_k+v_k,\hfill \end{array}$$

(1b)

where the state vector $x_k$ and the measurement vector $y_k$ at time step k belong to an open subset of ${\mathbb{R}}^n\times {\mathbb{R}}^m$; $F_{k-1}\in {\mathbb{R}}^{n\times n}$ is the invertible transition matrix. It is assumed that only part of the state vector is directly measured from a set of sensors, indicating that $m<n$ and leading to an additional assumption in which the known measurement matrix $H_k\in {\mathbb{R}}^{m\times n}$ has full row rank. In addition, both the state and measurement vectors are perturbed by an uncorrelated zero-mean white Gaussian process noise $w_{k-1}\sim \mathcal{N}\left(0,Q_{k-1}\right)$ and measurement noise $v_k\sim \mathcal{N}\left(0,R_k\right)$, respectively.

Suppose now that only some components of the state vector should satisfy some nonlinear equality constraints at time step k such that

$$\begin{array}{c}\hfill g\left(x_k\right)=d,\end{array}$$

(2)

where $g\left(x_k\right)\in {\mathbb{R}}^s$ is a nonlinear state-dependent function, its dimension s represents the number of constraints, and d is a known constant vector. As suggested by Simon [10], to incorporate (2) into the linear KF framework, one may linearize it around the best current KF’s estimate, which could be obtained after the measurement update step. In other words, the above nonlinear constraint can be linearized using the first-order Taylor expansion around the a posteriori estimate ${\widehat{x}}_k^{+}$ as follows:

$$\begin{array}{c}\hfill g\left(x_k\right)\approx D_k{x}_k=d_k,\end{array}$$

(3)

where $D_k={\displaystyle \frac{\partial g\left(x_k\right)}{\partial x_k}}{|}_{x_k={\widehat{x}}_k^{+}}$ and $d_k=d-g\left({\widehat{x}}_k^{+}\right)+D_k{\widehat{x}}_k^{+}$. Here, the constraint matrix $D_k$ is a state-dependent matrix. Moreover, in this paper, we are interested in the situation where $D_k$ has full rank and $\mathrm{rank}\left(D_k\right)=s\le n$ [10].

The goal is to recursively find a constrained estimate of $x_k$ at time step k denoted by ${\tilde{x}}_k$ given the current unconstrained estimate ${\widehat{x}}_k^{+}$ and the linearized constraints (3). That is to say, we seek the mean value of a conditional distribution ${\tilde{x}}_k=\mathrm{E}[x_k\left|{\widehat{x}}_k^{+},d_k\right]$ from the following optimization problem (OP) [10]:

$$\begin{array}{cc}\hfill \underset{x_k\in {\mathbb{R}}^n}{\mathrm{minimize}}& {\left({\tilde{x}}_k-{\widehat{x}}_k^{+}\right)}^{\top }{\left(P_k^{+}\right)}^{-1}\left({\tilde{x}}_k-{\widehat{x}}_k^{+}\right),\hfill \end{array}$$

(4a)

$$\begin{array}{cc}\hfill \mathrm{subject}\mathrm{to}& D_k{\tilde{x}}_k=d_k.\hfill \end{array}$$

(4b)

where the vector ${\widehat{x}}_k^{+}$ is the a posteriori state estimate of the KF and its corresponding covariance matrix (mean squares) $P_k^{+}$. The above OP is formulated by directly projecting the unconstrained state estimation onto the constraint surface (estimate projection method [9]), and the solution to this problem, if the constraint (4b) is feasible, is then given as follows [10]:

$$\begin{array}{cc}\hfill {\tilde{x}}_k& ={\widehat{x}}_k^{+}-P_k^{+}{D}_k^{\top }{\left(D_k{P}_k^{+}{D}_k^{\top }\right)}^{-1}\left(D_k{\widehat{x}}_k^{+}-d_k\right)\hfill \end{array}$$

(5a)

$$\begin{array}{cc}\hfill {\tilde{P}}_k& =P_k^{+}-P_k^{+}{D}_k^{\top }{\left(D_k{P}_k^{+}{D}_k^{\top }\right)}^{-1}{D}_k{P}_k^{+},\hfill \end{array}$$

(5b)

where ${\tilde{P}}_k$ is the covariance matrix of ${\tilde{x}}_k$. These calculations are referred to as the CKF. In the above expression, the vector ${\widehat{x}}_k^{+}$ can be obtained from the following procedure ([24], pp. 128–129):

$$\begin{array}{cc}\hfill {\widehat{x}}_k^{-}& =F_{k-1}{\widehat{x}}_{k-1}^{+}\hfill \end{array}$$

(6a)

$$\begin{array}{cc}\hfill P_k^{-}& =F_{k-1}{P}_{k-1}^{+}{F}_{k-1}^{\top }+Q_{k-1}\hfill \end{array}$$

(6b)

$$\begin{array}{cc}\hfill K_k& =P_k^{-}{H}_k^{\top }{\left(H_k{P}_k^{-}{H}_k^{\top }+R_k\right)}^{-1}\hfill \end{array}$$

(6c)

$$\begin{array}{cc}\hfill {\widehat{x}}_k^{+}& ={\widehat{x}}_k^{-}+K_k\left(y_k-H_k{\widehat{x}}_k^{-}\right)\hfill \end{array}$$

(6d)

$$\begin{array}{cc}\hfill P_k^{+}& =\left(I-K_k{H}_k\right)P_k^{-}{(I-K_k{H}_k)}^{\top }+K_k{R}_k{K}_k^{\top },\hfill \end{array}$$

(6e)

where $P_k^{-}$ is the covariance matrix of the a priori state estimate ${\widehat{x}}_k^{-}$ of the KF. It is worth noting that we choose the Joseph formula to calculate $P_k^{+}$ as in (6e) for improving numerical stability ([25], Section 5.2.6, p. 190).

3.3. Error-State Kalman Filter

Defining the error-state estimate as $\delta x_k=x_k-{\widehat{x}}_k$, the system in (1) can be reformulated as the following error dynamics model:

$$\begin{array}{cc}\hfill \delta x_k& =F_{k-1}\delta x_{k-1}+w_{k-1}\hfill \end{array}$$

(7a)

$$\begin{array}{cc}\hfill \delta y_k& =H_k\delta x_k+v_k.\hfill \end{array}$$

(7b)

In this section, to avoid introducing additional symbols, we shall abuse notation by using the same symbols in Section 3.2 to share the same denotation but for the error state (e.g., $w_{k-1}$ and $v_k$).

The procedure to find the optimal error estimate for the above system using KF is referred to as the error-state Kalman filter and can be generalized as follows [26] ([24], Section 13.1):

$$\begin{array}{cc}\hfill P_k^{-}& =F_{k-1}{P}_{k-1}^{+}{F}_{k-1}^{\top }+Q_{k-1}\hfill \end{array}$$

(8a)

$$\begin{array}{cc}\hfill K_k& =P_k^{-}{H}_k^{\top }{\left(H_k{P}_k^{-}{H}_k^{\top }+R_k\right)}^{-1}\hfill \end{array}$$

(8b)

$$\begin{array}{cc}\hfill \delta {\widehat{x}}_k^{+}& =K_k\left(y_k-H_k{\widehat{x}}_k^{-}\right)\hfill \end{array}$$

(8c)

$$\begin{array}{cc}\hfill P_k^{+}& =\left(I-K_k{H}_k\right)P_k^{-}{(I-K_k{H}_k)}^{\top }+K_k{R}_k{K}_k^{\top }\hfill \end{array}$$

(8d)

$$\begin{array}{cc}\hfill {\widehat{x}}_k^{+}& =F_{k-1}{\widehat{x}}_{k-1}^{+}+\delta {\widehat{x}}_k^{+}.\hfill \end{array}$$

(8e)

Compared to the direct KF’s formula in (6), the ErKF intentionally set the a priori error-state estimate $\delta {\widehat{x}}_k^{-}$ as zero at the beginning of every time step ([12], p. 406). It is implied that the a posteriori error-state estimate of the previous step $\delta {\widehat{x}}_{k-1}^{+}$ is also forced to zeros. This is because the correction phase in (8e) explicitly transfers the estimation error to the actual state as a closed-loop KF ([27], Section 3.2.6).

4. Error-State Kalman Filter with Linearized Constraints

4.1. Problem and Solution Formulation

Henceforth, the term unconstrained ErKF (unC-ErKF) will be defined as being interchangeable with the conventional ErKF that has been discussed in the previous section. Due to the presence of an error correction step, incorporating constraints into the unC-ErKF algorithm using the projection method might not have a unique solution. For this reason, we shall focus our efforts on identifying the following two possible approaches:

• Pre-constrained ErKF (PreC-ErKF): This method enforces the error state before the correction step. Thus, the OP will be formulated around the a priori state estimate ${\widehat{x}}_k^{-}$ since the a posteriori state estimate ${\widehat{x}}_k^{+}$ is not yet calculated. Moreover, in this case, non-zero ErKF estimates will be projected onto the constraint surface.
• Post-constrained ErKF (PostC-ErKF): In contrast to the above approach, the postC-ErKF enforces the constraints after the correction step. Consequently, the OP will now be computed with ${\widehat{x}}_k^{+}$, and the zero-valued ErKF’s error state will be projected onto the constraint surface due to the value reset procedure.

The above two constrained ErKFs are described with the aid of a block diagram, as depicted in Figure 1. One can notice that since the ErKF’s estimates are reset after the correction (i.e., $\delta {\widehat{x}}_k^{+}=0$), their statistical properties will be altered noticeably [13,28]. Therefore, constraint enforcement before and after the stage could provide divergent solutions, and they need to be rigorously studied.

Since this paper considers nonlinear constraints, we need to linearize them around the current estimated state to derive the corresponding error-state constraints. Assume that the error state $\delta x_k$ is sufficiently small so that the high-order terms of the linearization can be ignored. Thus, from (2) and the preceding definition of $\delta x$ and $D_k$, we have

$$\begin{array}{cc}\hfill {\displaystyle \frac{\partial g\left(x_k\right)}{\partial x_k}}{|}_{x={\widehat{x}}_k}\delta x_k& =d-g\left({\widehat{x}}_k\right)=D_k\delta x_k=\delta d_k,\hfill \end{array}$$

(9)

where the description of $\delta d_k$ is apparent from the above equation. To support our proposal, as established earlier in this section, and to enhance the clarity of the development below, one needs to further define

$$\begin{array}{c}\hfill D_k^{-}={\displaystyle \frac{\partial g\left(x_k\right)}{\partial x_k}}{|}_{x_k={\widehat{x}}_k^{-}},\delta d_k^{-}=d-g\left({\widehat{x}}_k^{-}\right),\end{array}$$

(10)

and

$$\begin{array}{c}\hfill D_k^{+}={\displaystyle \frac{\partial g\left(x_k\right)}{\partial x_k}}{|}_{x_k={\widehat{x}}_k^{+}},\delta d_k^{+}=d-g\left({\widehat{x}}_k^{+}\right).\end{array}$$

(11)

The above $D_k^{-}$ and $D_k^{+}$ are, respectively, used for the preC-ErKF and postC-ErKF. The occurrence of this discrepancy can be attributed to the fact that in the case of the pre-constrained approach, at the time of calculating $D_k$, only the a priori estimate ${\widehat{x}}_k^{-}$ is available. On the contrary, since the postC-ErKF enforces the constraints after the correction, the a posteriori estimate ${\widehat{x}}_k^{+}$ is obtainable for calculating $D_k$ so that it becomes $D_k^{+}$.

One could indicate that the estimate $D_k^{-}$ and $D_k^{+}$ of $D_k$ are calculated using different state estimates. Hypothetically speaking, since the a posteriori estimate ${\widehat{x}}_k^{+}$ has a smaller error covariance than that of the a priori estimate ${\widehat{x}}_k^{-}$ [5], we may reasonably argue that using the former could reduce the linearization error of $D_k$. This argument is implicitly consistent with the findings presented in [11]. For subsequent analysis, we introduce the following mathematical derivations of the techniques mentioned earlier:

Lemma 1 

(PreC-ErKF). Given the ErKF’s a posteriori estimate $\delta {\widehat{x}}_k^{+}$ at time step k, finding the pre-constrained error-state estimate $\delta {\tilde{x}}_k^{-}$ involves the following OP:

$$\begin{array}{cc}\hfill \delta {\tilde{x}}_k^{-}=\underset{\delta {\tilde{x}}_k\in {\mathbb{R}}^n}{\arg \min }& {\left(\delta {\tilde{x}}_k-\delta {\widehat{x}}_k^{+}\right)}^{\top }{\left(P_k^{+}\right)}^{-1}\left(\delta {\tilde{x}}_k-\delta {\widehat{x}}_k^{+}\right),\hfill \end{array}$$

(12a)

$$\begin{array}{cc}\hfill \mathrm{subject}\mathrm{to}& D_k^{-}\delta {\tilde{x}}_k=\delta d_k^{-},\hfill \end{array}$$

(12b)

and its solution

$$\begin{array}{cc}\hfill \delta {\tilde{x}}_k^{-}& =\delta {\widehat{x}}_k^{+}-W_k^{-}\left(D_k^{-}\delta {\widehat{x}}_k^{+}-\delta d_k^{-}\right)\hfill \end{array}$$

(13a)

$$\begin{array}{cc}\hfill {\tilde{x}}_k^{-}& ={\widehat{x}}_k^{-}+\delta {\tilde{x}}_k^{-},\hfill \end{array}$$

(13b)

where $W_k^{-}=P_k^{+}{D}_k^{-\top }{\left(D_k^{-}{P}_k^{+}{D}_k^{-\top }\right)}^{-1}$ and the vector ${\tilde{x}}_k^{-}$ is the final estimate obtained by correcting the constrained error estimate $\delta {\tilde{x}}_k^{-}$ from the KF’s a priori estimate ${\widehat{x}}_k^{-}$. Note that the estimation characteristics (e.g., mean and error covariance) of ${\tilde{x}}_k^{-}$ and ${\widehat{x}}_k^{-}$ are distinct.

Proof. 

The constrained problem of the error state can be directly derived from the estimate projection method in Section 3.2. Particularly, (4a) can be rewritten as

$$\begin{array}{c}\hfill \left(-{\left[\left(x_k-{\tilde{x}}_k^{-}\right)-\left(x_k-{\widehat{x}}_k^{+}\right)\right]}^{\top }\right){\left(P_k^{+}\right)}^{-1}\left(-\left[\left(x_k-{\tilde{x}}_k^{-}\right)-\left(x_k-{\widehat{x}}_k^{+}\right)\right]\right).\end{array}$$

(14)

Let $\delta {\tilde{x}}_k^{-}=x_k-{\tilde{x}}_k^{-}$ be the difference between the actual state and the constrained estimate. It can be verified, by the definition of the true error $\delta x_k$ in Section 3.3, that $\delta {\widehat{x}}_k^{+}=x_k-{\widehat{x}}_k^{+}$. These together yield (12a). Next, by the fact that after calculating $\delta {\tilde{x}}_k^{-}$, we need to correct the constrained estimation error to find the constrained estimate as in (13b). By substituting ${\tilde{x}}_k^{-}$ into (4b) and replacing the linearization point of $d_k$ to the a priori estimate ${\widehat{x}}_k^{-}$, we obtain (12b). Now, combining (12a) with (12b), we formulate an estimate projection problem for the error state. One may use the method of Lagrange multipliers to solve this problem. By doing so, the solution should be the same as in (13b). The step-by-step derivation of this algorithm can be found in ([10], Section III).

Alternatively, the solution can be directly obtained from the direct CKF result in (5a). Specifically, by substituting (5a) into (13b), we obtain

$$\begin{array}{c}\hfill {\widehat{x}}_k^{+}-W_k^{-}(D_k^{-}{\widehat{x}}_k^{+}-d_k)={\widehat{x}}_k^{-}+\delta {\tilde{x}}_k^{-}.\end{array}$$

(15)

By inserting (8e) into (15), we obtain

$$\begin{array}{c}\hfill {\widehat{x}}_k^{-}+\delta {\widehat{x}}_k^{+}-W_k^{-}\left[D_k^{-}\left({\widehat{x}}_k^{-}+\delta {\widehat{x}}_k^{+}\right)-d_k\right]={\widehat{x}}_k^{-}+\delta {\tilde{x}}_k^{-}.\end{array}$$

(16)

Canceling out the common factors and rearranging the terms, we have

$$\begin{array}{cc}\hfill \delta {\tilde{x}}_k^{-}& =\delta {\widehat{x}}_k^{+}-W_k^{-}\left[D_k^{-}\delta {\widehat{x}}_k^{+}-\left(d_k-D_k{\widehat{x}}_k^{-}\right)\right]\hfill \\ \hfill & =\delta {\widehat{x}}_k^{+}-W_k^{-}\left(D_k^{-}\delta {\widehat{x}}_k^{+}-\delta d_k^{-}\right).\hfill \end{array}$$

(17)

In the above expression, we have changed $g\left({\widehat{x}}_k^{+}\right)$ and $D_k{\widehat{x}}_k^{+}$ of $d_k$ in (3) to $g\left({\widehat{x}}_k^{-}\right)$ and $D_k^{-}{\widehat{x}}_k^{-}$, respectively. The reason is that the a posteriori estimate ${\widehat{x}}_k^{+}$ is not yet to be calculated until this step. We hence complete the proof of Lemma 1.    □

Remark 1. 

The PreC-ErKF is statistically consistent with the constrained Kalman gain methods in [22,29], particularly in that the constraint error is corrected based on the a priori estimate ${\widehat{x}}_k^{-}$. Interestingly, these two approaches are identical if the weighting factor $P_k^{+}$ in (12a) is intentionally set as an identity matrix [9].

Lemma 2 

(PostC-ErKF). Given that the a posteriori estimation error of KF has been corrected, i.e., ${\widehat{x}}_k^{+}$ is computed and $\delta {\widehat{x}}_k^{+}$ is reset to zero, finding the post-constrained error-state estimate $\delta {\tilde{x}}_k^{+}$ involves the following OP:

$$\begin{array}{cc}\hfill \delta {\tilde{x}}_k^{+}\underset{\delta {\tilde{x}}_k\in {\mathbb{R}}^n}{\mathrm{minimize}}& \delta {\tilde{x}}_k^{\top }{\left(P_k^{+}\right)}^{-1}\delta {\tilde{x}}_k,\hfill \end{array}$$

(18a)

$$\begin{array}{cc}\hfill \mathrm{subject}\mathrm{to}& D_k^{+}\delta {\tilde{x}}_k=\delta d_k^{+},\hfill \end{array}$$

(18b)

and its solution

$$\begin{array}{cc}\hfill \delta {\tilde{x}}_k^{+}& =W_k^{+}\delta d_k^{+}\hfill \end{array}$$

(19a)

$$\begin{array}{cc}\hfill {\tilde{x}}_k^{+}& ={\widehat{x}}_k^{+}+\delta {\tilde{x}}_k^{+},\hfill \end{array}$$

(19b)

where $W_k^{+}=P_k^{+}{D}_k^{+\top }{\left(D_k^{+}{P}_k^{+}{D}_k^{+\top }\right)}^{-1}$, and the vector ${\tilde{x}}_k^{+}$ is the final estimate obtained by correcting the constrained error estimate $\delta {\tilde{x}}_k^{+}$ from the KF’s a posteriori estimate ${\widehat{x}}_k^{+}$. The estimation characteristics of ${\tilde{x}}_k^{+}$ and ${\widehat{x}}_k^{+}$ are obviously different.

Proof 

(Sketch version). As the situation is analogous to Lemma 1, we give a sketch only. By substituting $\delta {\widehat{x}}_k^{+}=0$ into (12a) and (13a), we obtain (18a) and (19a), respectively. Note that, in this scenario, the error estimate from KF has been corrected so that we have ${\widehat{x}}_k^{+}$. Hence, we obtain the exact formulas of $D_k^{+}$ and $\delta d_k^{+}$ as in (11). Additionally, by substituting ${\tilde{x}}_k^{+}$ in (19b) to (4b), we directly have (18b) without any modification.    □

Remark 2. 

The result in (19a) shows that $\delta {\tilde{x}}_k^{+}=0$ if $\delta d_k^{+}=0$. This explicitly demonstrates that the unconstrained estimate ${\widehat{x}}_k^{+}$ satisfies the equality constraint in (2). Therefore, no more action (constraining) needs to be taken. This observation is more intelligible than the preC-ErKF approach. In particular, if the a priori unconstrained estimate satisfies the constraint, then from (13a), we have $\delta {\tilde{x}}_k^{-}=\delta {\widehat{x}}_k^{+}$, so that $D_k^{-}\delta {\widehat{x}}_k^{+}=\delta d_k^{-}$, which is equivalent to $D_k^{-}{\widehat{x}}_k^{+}=d_k$. This means that indeed, ${\widehat{x}}_k^{+}$ satisfies the direct constraint, not ${\widehat{x}}_k^{-}$.

4.2. Properties of the CErKF

In the following section, we will scrutinize some of the critical attributes of the preC-ErKF and postC-ErKF in the context of a linear constraint (i.e., $g\left(x_k\right)$ can be expressed as $D{x}_k$, where D is a constant matrix that has full rank s, and $d_k=d$). The motivation for this action is based on the fact that linearization makes it challenging to analyze the filter’s behavior theoretically. However, from the development of the linear case, we will establish a particular case of the nonlinear constraint $g\left(x_k\right)$, in which the effect of the linearization error is canceled out in the filter’s estimation confidence. Most of the developments below are formulated based on the full-state CKF results in [10] with appropriate adjustments.

Theorem 1. 

The constrained state estimates ${\tilde{x}}_k^{-}$ and ${\tilde{x}}_k^{+}$, as given by the pre-constrained and post-constrained ErKF, respectively, are both unbiased state estimators when the constraint is linear. That is,

$$\begin{array}{c}\hfill E\left[{\tilde{x}}_k^{-}\right]=E\left[{\tilde{x}}_k^{+}\right]=E\left[x_k\right].\end{array}$$

(20)

Proof. 

In this proof, we shall use the same symbol to denote a random variable and the value taken by it. There should be no confusion because of their distinctions.

Because the constraint is now a linear equation, we can rewrite the term $g\left({\widehat{x}}_k^{-}\right)$ in (10) as $D{\widehat{x}}_k^{-}$. As a clear consequence, we have $\delta d_k^{-}=d-D{\widehat{x}}_k^{-}$ and $W_k^{-}=W_k^{+}=W_k=P_k^{+}{D}^{\top }{\left(D{P}_k^{+}{D}^{\top }\right)}^{-1}$. From Lemma 1, it can be shown that

$$\begin{array}{cc}\hfill {\tilde{x}}_k^{-}& ={\widehat{x}}_k^{-}+\delta {\widehat{x}}_k^{+}-W_k\left(D\delta {\widehat{x}}_k^{+}-d+D{\widehat{x}}_k^{-}\right)\hfill \end{array}$$

(21)

$$\begin{array}{cc}\hfill & =\left(I_n-W_{k}D\right)\left({\widehat{x}}_k^{-}+\delta {\widehat{x}}_k^{+}\right)+W_{k}d\hfill \end{array}$$

(22)

$$\begin{array}{cc}\hfill & =\left(I_n-W_{k}D\right){\widehat{x}}_k^{+}+W_{k}d.\hfill \end{array}$$

(23)

Then,

$$\begin{array}{cc}\hfill x_k-{\tilde{x}}_k^{-}& =\left(I_n-W_{k}D\right)\left(x_k-{\widehat{x}}_k^{+}\right).\hfill \end{array}$$

(24)

Taking the expectation of both sides of the above equation gives

$$\begin{array}{cc}\hfill \mathrm{E}\left[x_k-{\tilde{x}}_k^{-}\right]& =\left(I_n-W_{k}D\right)\mathrm{E}\left[x_k-{\widehat{x}}_k^{+}\right].\hfill \end{array}$$

(25)

It should be noted that we take the random variable $W_k$ out of the expectation operator under the same assumptions as those used in the derivation of the EKF, specifically that $W_k$ is zero-mean [10,22].

In the similar manner, from Lemma 2, we can write

$$\begin{array}{cc}\hfill \mathrm{E}\left[x_k-{\tilde{x}}_k^{+}\right]& =\left(I_n-W_{k}D\right)\mathrm{E}\left[x_k-{\widehat{x}}_k^{+}\right].\hfill \end{array}$$

(26)

Since the unC-ErKF provides an unbiased estimate of $x_k$, thus, $\mathrm{E}\left[x_k-{\widehat{x}}_k^{+}\right]=0$. This implies that $\mathrm{E}\left[x_k-{\tilde{x}}_k^{-}\right]=\mathrm{E}\left[x_k-{\tilde{x}}_k^{+}\right]=0$.    □

Theorem 2. 

Let ${\tilde{P}}_k^{-}$ and ${\tilde{P}}_k^{+}$ be the error covariances of the constrained estimate before the correction ${\tilde{x}}_k^{-}$ and after the correction ${\tilde{x}}_k^{+}$, respectively. Suppose that the constraint is linear; then,

$$\begin{array}{c}\hfill {\tilde{P}}_k^{-}={\tilde{P}}_k^{+}<P_k^{+}.\end{array}$$

(27)

Proof. 

It can be shown from (25) that if the constraint is linear, we can write

$$\begin{array}{cc}\hfill {\tilde{P}}_k^{-}& =\mathrm{E}\left[\left(x_k-{\tilde{x}}_k^{-}\right){\left(x_k-{\tilde{x}}_k^{-}\right)}^{\top }\right]\hfill \end{array}$$

(28)

$$\begin{array}{cc}\hfill & =\left(I_n-W_{k}D\right)\mathrm{E}\left[\left(x_k-{\widehat{x}}_k^{+}\right){\left(x_k-{\widehat{x}}_k^{+}\right)}^{\top }\right]{\left(I_n-W_{k}D\right)}^{\top }\hfill \end{array}$$

(29)

$$\begin{array}{cc}\hfill & =\left(I_n-W_{k}D\right)P_k^{+}{\left(I_n-W_{k}D\right)}^{\top }.\hfill \end{array}$$

(30)

In a similar manner, from (26), we also have

$$\begin{array}{cc}\hfill {\tilde{P}}_k^{+}& =\mathrm{E}\left[\left(x_k-{\tilde{x}}_k^{+}\right){\left(x_k-{\tilde{x}}_k^{+}\right)}^{\top }\right]=\left(I_n-W_{k}D\right)P_k^{+}{\left(I_n-W_{k}D\right)}^{\top }.\hfill \end{array}$$

(31)

Clearly, one could show that

$$\begin{array}{cc}\hfill {\tilde{P}}_k^{-}={\tilde{P}}_k^{+}& =\left(I_n-W_{k}D\right)P_k^{+}{\left(I_n-W_{k}D\right)}^{\top }.\hfill \end{array}$$

(32)

Next, the above equation can be further expressed as

$$\begin{array}{c}\hfill {\tilde{P}}_k^{+}-P_k^{+}={\tilde{P}}_k^{-}-P_k^{+}=-W_{k}D{P}_k^{+}.\end{array}$$

(33)

It might be worth noting that (33) is obtained using the following equality:

$$\begin{array}{c}\hfill W_k{D}_k{P}_k^{+}=P_k^{+}{D}_k^{\top }{W}_k^{\top }=W_k{D}_k{P}_k^{+}{D}_k^{\top }{W}_k^{\top }.\end{array}$$

(34)

Since D is assumed to be full-rank (i.e., rank$\left(D\right)=s<n$), then $W_{k}D{P}_k^{+}$ is positive definite ([10], Th. 2). Thus, we can conclude that

$$\begin{array}{c}\hfill {\tilde{P}}_k^{+}<P_k^{+}\mathrm{and}{\tilde{P}}_k^{-}<P_k^{+}.\end{array}$$

(35)

By combining these results with (32), we complete the proof of Theorem 2.    □

Theorem 3. 

Let ${\Sigma }_k^{-}$ and ${\Sigma }_k^{+}$, respectively, be the covariances of the constrained estimation errors ${\tilde{x}}_k^{-}$ and ${\tilde{x}}_k^{+}$. If the constraint is linear, then the following equality holds:

$$\begin{array}{c}\hfill {\Sigma }_k^{+}={\Sigma }_k^{-}.\end{array}$$

(36)

Proof. 

The covariance of the pre-constrained estimation error $\delta {\tilde{x}}_k^{-}$ can be given as

$$\begin{array}{c}\hfill {\Sigma }_k^{-}=\mathrm{E}\left[\left({\tilde{x}}_k^{-}-\mathrm{E}\left[{\tilde{x}}_k^{-}\right]\right){\left({\tilde{x}}_k^{-}-\mathrm{E}\left[{\tilde{x}}_k^{-}\right]\right)}^{\top }\right].\end{array}$$

(37)

Recall that ${\tilde{x}}_k^{-}=x_k-\delta {\tilde{x}}_k^{-}$. Write

$$\begin{array}{cc}\hfill {\Sigma }_k^{-}& =\mathrm{E}\left[\left(x_k-\delta {\tilde{x}}_k^{-}-\mathrm{E}\left[x_k-\delta {\tilde{x}}_k^{-}\right]\right){\left(x_k-\delta {\tilde{x}}_k^{-}-\mathrm{E}\left[x_k-\delta {\tilde{x}}_k^{-}\right]\right)}^{\top }\right].\hfill \end{array}$$

(38)

By Theorem 1, we know that $\mathrm{E}\left[\delta {\tilde{x}}_k^{-}\right]=0$ if the constraint is linear. Thus, the above equation becomes

$$\begin{array}{cc}\hfill {\Sigma }_k^{-}& =\mathrm{E}\left[\left(x_k-\delta {\tilde{x}}_k^{-}-\mathrm{E}\left[x_k\right]\right){\left(x_k-\delta {\tilde{x}}_k^{-}-\mathrm{E}\left[x_k\right]\right)}^{\top }\right]\hfill \end{array}$$

(39)

$$\begin{array}{cc}\hfill & =\mathrm{E}\left[\left(x_k-\mathrm{E}\left[x_k\right]\right){\left(x_k-\mathrm{E}\left[x_k\right]\right)}^{\top }\right]+\mathrm{E}\left[\delta {\tilde{x}}_k^{-}\delta {\tilde{x}}_k^{-\top }\right],\hfill \end{array}$$

(40)

where we use the fact that the constrained estimation error $\delta {\tilde{x}}_k^{-}$ is zero-mean and uncorrelated with the full state ([5], Ch. 5). Similarly, we can show that

$$\begin{array}{cc}\hfill {\Sigma }_k^{+}& =\mathrm{E}\left[\left(x_k-\mathrm{E}\left[x_k\right]\right){\left(x_k-\mathrm{E}\left[x_k\right]\right)}^{\top }\right]+\mathrm{E}\left[\delta {\tilde{x}}_k^{+}\delta {\tilde{x}}_k^{+\top }\right].\hfill \end{array}$$

(41)

Taking the difference between ${\Sigma }_k^{+}$ and ${\Sigma }_k^{-}$, we obtain

$$\begin{array}{cc}\hfill {\Sigma }_k^{+}-{\Sigma }_k^{-}& =\mathrm{E}\left[\delta {\tilde{x}}_k^{+}\delta {\tilde{x}}_k^{+\top }\right]-\mathrm{E}\left[\delta {\tilde{x}}_k^{-}\delta {\tilde{x}}_k^{-\top }\right]={\tilde{P}}_k^{+}-{\tilde{P}}_k^{-}.\hfill \end{array}$$

(42)

By Theorem 2, we can conclude that if the constraint is linear, then (32) holds, from which the result (36) follows.    □

Proposition 1. 

Suppose that the constraint is now nonlinear, and we need to linearize it to apply the results that have been derived so far. Let us consider the following circumstances:

1. 

If the nonlinear constraint can be expressed as

$$\begin{array}{c}\hfill g\left(x_k\right)=c{D}_k{x}_k=d,\end{array}$$

(43)

where c is a constant scalar, and the state-dependent matrix $D_k=D\left(x_k\right)$ is obtained from linearizing $g\left(x_k\right)$, then Theorem 1 and (35) hold.

2. 

If the above linearized constraint matrix $D_k$ can be further written as

$$\begin{array}{c}\hfill D_k=C\circ u\left(x_k\right)1_n^{\top },\end{array}$$

(44)

where $C\in {\mathbb{R}}^{s\times n}$ is a constant matrix, and the vector $u\left(x_k\right)\in {\mathbb{R}}^s$ contains state-dependent functions such that $f:{\mathbb{R}}^n\to \mathbb{R}\setminus \left\{0\right\}$, then, not only are both of the considered filters unbiased estimators, but also their corresponding constrained error covariances ${\tilde{P}}_k^{-}$ and ${\tilde{P}}_k^{+}$ are identical. Consequently, it can be inferred from the proof of Theorem 3 that ${\Sigma }_k^{-}$ and ${\Sigma }_k^{+}$ are also identical.

Proof. 

We first provide an illustrative example to enhance the clarity of the subsequent discussion. Suppose that the state variable $x_k={\left[\begin{array}{ccc}{x}_1& x_2& x_3\end{array}\right]}^{\top }\in {\mathbb{R}}^3$ and it satisfies the following polynomial equations: $g\left(x_k\right)={\left[\begin{array}{cc}{x}_1^2& x_2^2\end{array}\right]}^{\top }$. This constraint fulfills both (43) and (44).

Remark 3. 

Since the system’s constraints not only come from the physical laws but can also be chosen and designed by engineers, it is possible for the nonlinear constraint $g\left(x_k\right)$ to meet one of those assumptions in practice.

Now, assuming that (43) holds, one could rewrite it as

$$\begin{array}{c}\hfill g\left(x_k\right)=D_k{x}_k={\displaystyle \frac{d}{c}}.\end{array}$$

(45)

Because the constraint is nonlinear, (21) becomes

$$\begin{array}{cc}\hfill {\tilde{x}}_k^{-}& ={\widehat{x}}_k^{-}+\delta {\widehat{x}}_k^{+}-W_k^{-}\left(D_k^{-}\delta {\widehat{x}}_k^{+}-{\displaystyle \frac{d}{c}}+g\left({\widehat{x}}_k^{-}\right)\right)\hfill \end{array}$$

(46)

$$\begin{array}{cc}\hfill & ={\widehat{x}}_k^{+}-W_k^{-}\left(D_k^{-}{\widehat{x}}_k^{+}-D_k^{-}{\widehat{x}}_k^{-}-{\displaystyle \frac{d}{c}}+D_k^{-}{\widehat{x}}_k^{-}\right)\hfill \end{array}$$

(47)

$$\begin{array}{cc}\hfill & =\left(I_n-W_k^{-}{D}_k^{-}\right){\widehat{x}}_k^{+}+{\displaystyle \frac{1}{c}}{W}_k^{-}d.\hfill \end{array}$$

(48)

Similarly, one could obtain

$$\begin{array}{cc}\hfill {\tilde{x}}_k^{+}& =\left(I_n-W_k^{+}{D}_k^{+}\right){\widehat{x}}_k^{+}+{\displaystyle \frac{1}{c}}{W}_k^{+}d.\hfill \end{array}$$

(49)

Clearly, for the similar rationale presented in the proof of Theorem 1, it follows that $\mathrm{E}\left[x_k-{\tilde{x}}_k^{-}\right]=\mathrm{E}\left[x_k-{\tilde{x}}_k^{+}\right]=0$. This means the preC-ErKF and the postC-ErKF are both unbiased estimators.

From (48), (49), and (33), it can be verified that

$$\begin{array}{c}\hfill {\tilde{P}}_k^{+}-P_k^{+}=-W_k^{+}{D}_k^{+}{P}_k^{+}\end{array}$$

(50)

and

$$\begin{array}{c}\hfill {\tilde{P}}_k^{-}-P_k^{+}=-W_k^{-}{D}_k^{-}{P}_k^{+}.\end{array}$$

(51)

Since $W_k^{+}{D}_k^{+}{P}_k^{+}$ and $W_k^{-}{D}_k^{-}{P}_k^{+}$ are both positive definite for all $D_k^{-}$ and $D_k^{+}$ ([10], Th. 2), (35) holds.

Next, it follows from (50) and (51) that

$$\begin{array}{cc}\hfill {\tilde{P}}_k^{+}-{\tilde{P}}_k^{-}& =W_k^{-}{D}_k^{-}{P}_k^{+}-W_k^{+}{D}_k^{+}{P}_k^{+}.\hfill \end{array}$$

(52)

If $D_k$ satisfies (44), from the definition of $W_k^{+}$ in Lemma 2, we have

$$\begin{array}{cc}\hfill {\tilde{P}}_k^{+}& =P_k^{+}-W_k^{+}{D}_k^{+}{P}_k^{+}\hfill \end{array}$$

(53)

$$\begin{array}{cc}\hfill & =P_k^{+}-P_k^{+}{D}_k^{+\top }{\left(D_k^{+}{P}_k^{+}{D}_k^{+\top }\right)}^{-1}{D}_k^{+}{P}_k^{+}\hfill \end{array}$$

(54)

$$\begin{array}{cc}\hfill & =P_k^{+}-P_k^{+}{\left(C\circ u\left({\widehat{x}}_k^{+}\right)1_n^{\top }\right)}^{\top }{\left(\left(C\circ u\left({\widehat{x}}_k^{+}\right)1_n^{\top }\right)P_k^{+}{\left(C\circ u\left({\widehat{x}}_k^{+}\right)1_n^{\top }\right)}^{\top }\right)}^{-1}\left(C\circ u\left({\widehat{x}}_k^{+}\right)1_n^{\top }\right)P_k^{+}.\hfill \end{array}$$

(55)

Notice that the Hadamard product terms in (55) can be rewritten as $C\circ u\left({\widehat{x}}_k^{+}\right)1_n^{\top }=\mathrm{diag}\left(u\left({\widehat{x}}_k^{+}\right)\right)C$, where the operator $\mathrm{diag}:{\mathbb{R}}^n\to {\mathbb{R}}^{n\times n}$ returns a square diagonal matrix.

Remark 4. 

The range of function f guarantees that the matrix $\mathit{diag}\left(u\left({\widehat{x}}_k^{+}\right)\right)$ is not singular. In fact, this is equivalent to having a full-rank $D_k$, which coincides with the assumption in Section 3.2. Thus, the condition is trivially met.

By substituting this equality into (55), we obtain

$$\begin{array}{cc}\hfill {\tilde{P}}_k^{+}& =P_k^{+}-P_k^{+}{\left(\mathrm{diag}\left(u\left({\widehat{x}}_k^{+}\right)\right)C\right)}^{\top }{\left(\mathrm{diag}\left(u\left({\widehat{x}}_k^{+}\right)\right)C{P}_k^{+}{\left(\mathrm{diag}\left(u\left({\widehat{x}}_k^{+}\right)\right)C\right)}^{\top }\right)}^{-1}\mathrm{diag}\left(u\left({\widehat{x}}_k^{+}\right)\right)C{P}_k^{+}\hfill \end{array}$$

(56)

$$\begin{array}{cc}\hfill & =P_k^{+}-P_k^{+}{C}^{\top }{\left(C{P}_k^{+}{C}^{\top }\right)}^{-1}C{P}_k^{+}.\hfill \end{array}$$

(57)

It can be seen that the above equation is equivalent to (32), meaning that the effect of the linearization error from $D_k^{+}$ to the post-constrained error covariance ${\tilde{P}}_k^{+}$ is eliminated. This results in a similar error covariance ${\tilde{P}}_k^{-}$ of the pre-constrained approach, where the effect of the linearization error from $D_k^{-}$ is also canceled out. Hence, from (57), one can show that

$$\begin{array}{c}\hfill {\tilde{P}}_k^{+}-{\tilde{P}}_k^{-}=0_{n\times n}.\end{array}$$

(58)

From the proof of Theorem 3, we directly have ${\Sigma }_k^{+}-{\Sigma }_k^{-}=0_{n\times n}$.    □

4.3. Implementation

This section explains the step-by-step practical implementation of the filters in detail. To be specific, since the preC-ErKF and postC-ErKF share the same initialization and some portion of the time update processes, Algorithm 1 demonstrates their pseudocode jointly. It is worth mentioning that the choice of using the constrained process noise covariance ${\tilde{Q}}_0$ instead of $Q_0$ (lines 2 to 4 of Algorithm 1) is optional, even though it is recommended to use ${\tilde{Q}}_0$ as the initial value for the KF framework. By doing so, one could achieve significant enhancements (see

Table 1. Time-averaged RMS estimation (sum of all state’s elements) and constraint error of four considered filtering algorithms of the first example with two different constraints (complete and incomplete) over 600 Monte Carlo trials.

Filter Type	${\overline{\mathit{E}}}_{\mathbf{rms}}$		${\overline{\mathit{C}}}_{\mathbf{rms}}$
	${\mathbf{D}}_{\mathbf{1}}$	${\mathbf{D}}_{\mathbf{2}}$	${\mathbf{D}}_{\mathbf{1}}$	${\mathbf{D}}_{\mathbf{2}}$
Unconstrained ErKF ($Q_0$)	$24.141$	$24.085$	$32.560$	$4.495$
GP ErKF (${\tilde{Q}}_0$) [9]	$\mathbf{18}.\mathbf{373}$	$\mathbf{22}.\mathbf{480}$	$\mathbf{0}.\mathbf{0}$	$\mathbf{0}.\mathbf{0}$
Pre-constrained ErKF (${\tilde{Q}}_0$)	$\mathbf{18}.\mathbf{373}$	$22.516$	$\mathbf{0}.\mathbf{0}$	$\mathbf{0}.\mathbf{0}$
Post-constrained ErKF (${\tilde{Q}}_0$)	$\mathbf{18}.\mathbf{373}$	$\mathbf{22}.\mathbf{480}$	$\mathbf{0}.\mathbf{0}$	$\mathbf{0}.\mathbf{0}$

The numbers rendered in bold mark the minimum value, and all values are rounded to three decimal digits.

and ([9], Table II)). However, this technique [18] is not originated from the proposed filters; hence, it remains an option.

Algorithm 1 Constrained Error-state Kalman Filter

1: Initialize:   ${\widehat{x}}_0^{+}$, $P_0^{+}$, $Q_0$, $R_0$.   2: if Constrained ${\tilde{Q}}_0$ is chosen then   3:      From [18] and [9], it can be verified that ${\tilde{Q}}_0=N{Q}_{0}N$, where $N=U_2{U}_2^{\top }$ and $D_0^{\top }=\left[\begin{array}{cc}{U}_1& U_2\end{array}\right]\left[\begin{array}{cc}S& 0\end{array}\right]V^{\top }$.   4: end if   5: for $k=1$ to K do   6:     Time update ← (8a); $\delta {\widehat{x}}_k^{-}=0_{n\times 1}$.   7:     if Measurement is available then   8:         Measurement update ← From (8b) to (8d).

Option #1: Pre-constrained

9:         Compute $D_k^{-}$ and $\delta d_k^{-}$ with ${\widehat{x}}_k^{-}$. 10:         if $D_k^{-}\delta {\widehat{x}}_k^{-}\ne \delta d_k^{-}$ then 11:            Constraint ← Calculate $\delta {\tilde{x}}_k^{-}$ as in (13a). 12:            Correction ← Calculate ${\tilde{x}}_k^{-}$ as in (13b). 13:         else 14:            Unconstrained correction ← Calculate ${\widehat{x}}_k^{+}$ as in (8e). 15:            Reset $\delta {\widehat{x}}_k^{+}=0_{n\times 1}$. 16:            ${\tilde{x}}_k^{-}={\widehat{x}}_k^{+}$. 17:         end if

Option #2: Post-constrained

18:         Unconstrained correction ← Calculate ${\widehat{x}}_k^{+}$ as in (8e). 19:         Compute $D_k^{+}$ and $\delta d_k^{+}$ with ${\widehat{x}}_k^{+}$. 20:         Reset $\delta {\widehat{x}}_k^{+}=0_{n\times 1}$. 21:         if $\delta d_k^{+}\ne 0_{s\times 1}$ (see Remark 2) then 22:            Constraint ← Calculate $\delta {\tilde{x}}_k^{+}$ as in (19a). 23:            Correction ← Calculate ${\tilde{x}}_k^{+}$ as in (19b). 24:         else 25:            ${\tilde{x}}_k^{+}={\widehat{x}}_k^{+}$. 26:         end if 27:     else 28:         ${\widehat{x}}_k^{+}={\widehat{x}}_k^{-}$, $P_k^{+}=P_k^{-}$. 29:     end if 30: end for

As pointed out in Figure 1, the difference between those two methods is the number of correction steps. In cases where constraining is deemed necessary, the postC-ErKF mandates the implementation of two correction steps, one for the unconstrained estimate (8e) and another for the constrained estimate (19b). In contrast, the preC-ErKF only necessitates a single correction step (8e) or (13b), irrespective of the extent to which the constraints are satisfied. It is important to note that the ultimate solutions ${\tilde{x}}_k^{-}$ and ${\tilde{x}}_k^{+}$ of the mentioned filters are not fed back into the conventional ErKF’s mainstream. Notwithstanding the lack of verification, we firmly believe that propagating a constrained estimate through an unconstrained model may have a negative impact on the ErKF’s statistical characteristics and behavior.

5. Case Studies

To rigorously validate the performance of the two investigated filters in this paper, we reproduce two test case problems in [9] with increasing initial condition uncertainties. The suitability of these two problems as validation benchmarks for the filters can be attributed to their simple system models. However, they can still reflect most of the filter’s salient behavior. Moreover, these testing scenarios are frequently encountered in practical applications, and their results can be employed to align summary outcomes in the cited survey paper. For the sake of clarity, we have preserved the exact values of all parameters utilized in both simulations except the number of Monte Carlo runs and have repeated them for the reader’s benefit.

The primary objective of this section is to analyze the numerical performance of the preC-ErKF and postC-ErKF to determine the more effective approach for incorporating constraints into the ErKF when the EPM is chosen. Additionally, we compare these methods with the UnC-ErKF and the gain projection ErKF (GP-ErKF) presented in [9]. This comparison is motivated by the fact that, despite being mentioned in [9], the GPM was neither considered nor numerically evaluated. By including this analysis, we aim to provide a more comprehensive comparison of constrained filtering techniques.

The metric for comparison between approaches is only accuracy. Note that since they are both KF-based algorithms, the consistency of the filters can be theoretically guaranteed if KF’s process noise $Q_k$ and measurement $R_k$ noise covariance matrices are well chosen. Thus, we intentionally omit the filter’s consistency analysis. For evaluating the accuracy of the estimates, a Monte Carlo-averaged root mean square estimation error (RMSEE) $E_{rms,k}$ is considered [30]. The value of $E_{rms,k}$ is computed over a set of M given Monte Carlo trials as $E_{rms,k}=\sqrt{{\displaystyle \frac{1}{M}}{\sum }_{m=1}^M{∥x_k^m-{\widehat{x}}_k^m∥}_2^2}$. Here, $x_k^m$ and ${\widehat{x}}_k^m$ are the actual and estimated states at the time instant k during the m-th Monte Carlo run. From this, the time-averaged RMSEE ${\overline{E}}_{rms,k}$ can be calculated as ${\overline{E}}_{rms,k}={\displaystyle \frac{1}{K}}{\sum }_{k=1}^K{E}_{rms,k}$. The constraint error is also taken into account in terms of the RMS error [10]. Specifically, the RMS constraint error (RMSCE) is defined as $C_{rms,k}=\sqrt{{\displaystyle \frac{1}{M}}{\sum }_{m=1}^M{∥D_k^m{x}_k^m-{\widehat{D}}_k^m{\widehat{x}}_k^m∥}_2^2}$. Subsequently, the time-averaged RMSCE is computed as ${\overline{C}}_{rms,k}={\displaystyle \frac{1}{K}}{\sum }_{k=1}^K{C}_{rms,k}$.

5.1. Example 1: A Linear System with Linear Equality Constraints

In this example, we study a simple position estimation of a two-dimensional (2D) vehicle. The state vector $x_k={\left[\begin{array}{cccc}{p}_{n,k}& p_{e,k}& v_{n,k}& v_{e,k}\end{array}\right]}^{\top }$ contains the position and velocity of the vehicle in the east (x-axis) and north (y-axis) directions, respectively. The state space error dynamical model for this system can be deduced from its full-state form as

$$\begin{array}{cc}\hfill \delta x_k& =\left[\begin{array}{cccc}1& 0& \Delta t& 0\\ 0& 1& 0& \Delta t\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right]\delta x_{k-1}+w_{k-1}\hfill \end{array}$$

(59a)

$$\begin{array}{cc}\hfill \delta y_k& =\left[\begin{array}{cccc}1& 0& 0& 0\\ 0& 1& 0& 0\end{array}\right]\delta x_k+v_k,\hfill \end{array}$$

(59b)

where $\Delta t$ is the sample period of the filter. Note that to obtain the above system from the original full-state dynamical equation, we assume that the control input is perfectly known. Next, the constraint on the error state is given as

$$\begin{array}{c}\hfill \left[\begin{array}{cccc}1& -tan\theta & 0& 0\\ 0& 0& 1& -tan\theta \end{array}\right]\delta x_k=D_1\delta x_k=\delta d_{1,k},\end{array}$$

(60)

where $\theta$ is the vehicle’s heading angle measured counter-clockwise from due east, and the error constraint value is written as $\delta d_{1,k}=d_1-D_1{\widehat{x}}_k$. To conduct a more comprehensive analysis, we additionally employ an inconsistent variant of $D_1$ where only the vehicle’s velocity is constrained. In particular, this constraint can be written as follows:

$$\begin{array}{c}\hfill D_2=\left[\begin{array}{cccc}0& 0& 1& -tan\theta \end{array}\right].\end{array}$$

(61)

This constraint is considered as an incomplete one. In contrast to the constraint $D_1$, the constraint $D_2$ relies on the principle that velocity determines position, implying that a velocity constraint implicitly constrains position. However, since position is derived from velocity through an approximate integration, any error in the velocity estimation could adversely affect the position constraint. For a detailed discussion, please refer to [9] and the relevant literature mentioned therein.

For the system model’s parameters, the heading angle $\theta$ is chosen as a constant of 60 degrees, and the constraint values on the full state, $d_1$ and $d_2$, are set to ${\left[\begin{array}{cc}0& 0\end{array}\right]}^{\top }$ and 0, respectively. For the filter’s parameters, the sample period $\Delta t$ is set to 3 s, the KF’s initial state and its corresponding covariance are ${\widehat{x}}_0^{+}=$ and $P_0^{+}=\mathrm{diag}\left(900,900,4,4\right)$. The process noise ${\omega }_{k-1}$ and measurement noise $v_k$ are presumed to have the same statistical characteristics as considered in Section 3.2 with the value of their noise covariance matrices being $Q_k=\mathrm{diag}\left(4,4,1,1\right)$ and $R_k=\mathrm{diag}\left(900,900\right)$. We implement all the algorithms (unC-ErKF, GP-ErKF, preC-ErKF, and postC-ErKF) on a 150 s simulation over 600 Monte Carlo trials for two scenarios where $D_1$ and $D_2$ are individually considered.

Table 1 lists the time-averaged RMSEE ${\overline{E}}_{rms,k}$ and RMSCE ${\overline{C}}_{rms,k}$ obtained from the above-mentioned simulation. Two observations can be drawn from these results. Firstly, the preC-ErKF, postC-ErKF, and GP-ErKF together outperform the UnC-ErKF as can be easily predicted both intuitively and mathematically from the result of Theorem 2. Secondly, when using $D_1$, the estimation and constraint errors of all frameworks, except for the UnC-ErKF, are indistinguishable. This aligns with the findings in [9], which state that all the constrained filters yield identical performance if the constraint is complete.

In the case of the incomplete constraint matrix $D_2$, although evaluated approaches achieve comparable levels of constraint satisfaction efficacy, the postC-ErKF provides slightly better improvement compared to the preC-ErKF and is identical to the GP-ErKF. Based on this result, one may tentatively conclude that the preC-ErKF is more sensitive to constraint incompleteness than others.

It is also worth emphasizing that unlike in [9], we compared the unC-ErKF with $Q_0$ and the C-ErKFs with ${\tilde{Q}}_0$. This implementation is based on the principle that the unconstrained filter should completely ignore the constraint’s information. The results presented in Table 1 provide partial corroboration of the theorems that we have proven previously.

5.2. Example 2: A Nonlinear System with a Nonlinear Equality Constraint

In the following, the investigated filters are employed to estimate a nonlinear pendulum with a nonlinear energy constraint [31]. We linearize the system using the first-order Taylor expansion to deal with the nonlinearity. Hence, the error-state model of the pendulum can be expressed as follows:

$$\begin{array}{cc}\hfill \left[\begin{array}{c}\delta {\varphi }_k\\ \delta {\Omega }_k\end{array}\right]& =\left[\begin{array}{cc}1& \Delta t\\ -{\displaystyle \frac{g\Delta tcos\left({\varphi }_{k-1}\right)}{L}}& 1\end{array}\right]\left[\begin{array}{c}\delta {\varphi }_{k-1}\\ \delta {\Omega }_{k-1}\end{array}\right]+{\omega }_{k-1}\hfill \end{array}$$

(62a)

$$\begin{array}{cc}\hfill \delta y_k& =\left[\begin{array}{cc}1& 0\\ 0& 1\end{array}\right]\left[\begin{array}{c}\delta {\varphi }_k\\ \delta {\Omega }_k\end{array}\right]+v_k,\hfill \end{array}$$

(62b)

where $\delta {\varphi }_k$ and $\delta {\Omega }_k$ are the angular position and velocity error at the time instant k. The system’s state is updated at a time step of $\Delta t=0.05$ s. By conservation of energy, the system (62) satisfies the following first-order linearized constraint [9]:

$$\begin{array}{c}\hfill \left[\begin{array}{cc}mgLsin{\varphi }_k& m{L}^2{\Omega }_k\end{array}\right]\left[\begin{array}{c}\delta {\varphi }_k\\ \delta {\Omega }_k\end{array}\right]=\delta d_k.\end{array}$$

(63)

Here, $\delta d_k=C+mgLcos{\varphi }_k-{\displaystyle \frac{m{L}^2{\Omega }_k^2}{2}}$, where C is some constant. The system model’s parameters are chosen as follows: the gravitational force constant $g=9.81$ m/s², the pendulum length $L=1$ m, and the pendulum weight $m=1$ kg. The system’s initial state $x_0$ is set as ${\left[\begin{array}{cc}\pi /4& \pi /50\end{array}\right]}^{\top }$ with an update time step of $\Delta t=0.05$ s.

The filter’s process noise covariance matrix is chosen as $Q_k=\mathrm{diag}\left(0.{007}^2,0.{007}^2\right)$, and the measurement noise covariance matrix is $R_k=\mathrm{diag}\left(0.1^2,0.1^2\right)$. To further evaluate the filter’s performance, we analyze the robustness to bad initialization with an increasing initial condition error of ${\widehat{x}}_0^{+}$ for $25\%,50\%$, and $75\%$, respectively. Specifically, this error is mathematically defined as ${\widehat{x}}_0^{+}=(1-\beta )x_0$, where $\beta =\{0.25,0.5,0.75\}$. For this reason, the initial estimation error covariance will be set distinctly for each ${\widehat{x}}_0^{+}$. In particular, $P_0^{+}=\mathrm{E}\left[\left(x_0-{\widehat{x}}_0^{+}\right){\left(x_0-{\widehat{x}}_0^{+}\right)}^{\top }\right]$. The simulation was performed for a duration of 25 s, with 300 Monte Carlo trials executed.

Figure 2 demonstrates the square root of the actual estimation error covariance of ${\widehat{\varphi }}_k$ and ${\widehat{\Omega }}_k$ from Monte Carlo simulations. These results provide an initial intuitive assessment, suggesting that the theorems presented in this paper are not guaranteed to hold for general nonlinear systems. It is apparent from Figure 2a that when the initial error is relatively small ($\beta =0.25$), both proposed C-ErKF methods exhibit a smaller magnitude of the error covariance as compared to the conventional unC-ErKF at all time steps k. This finding is consistent with the inequality outlined in (27), despite the system being nonlinear and the constraint matrix failing to satisfy the assumptions of Proposition 1. Furthermore, despite the overall similarity in performance between the two considered methods, the post-constrained exposes a slightly better response during the early stages than its counterpart. Notably, the postC-ErKF also yields a smaller error covariance than the others, but the results may be difficult to discern from the figure. On the other hand, the GP-ErKF produces mixed results, exhibiting worse performance than the UnC-ErKF when $\beta =0.25$ and $\beta =0.5$. This may be due to the constraint imposed on the Kalman gain, which could excessively prioritize constraint satisfaction (see

Table 2. Time-averaged RMS estimation and constraint error of four considered filtering algorithms for the second example under $25\%$, $50\%$, and $75\%$ initial error test cases.

Filter Type	Time-Averaged RMS Estimation Error ($\widehat{\mathit{\varphi }}$, $\widehat{\mathbf{\Omega }}$)			Time-Averaged RMS Constraint Error
	0.25	0.5	0.75	0.25	0.5	0.75
UnC-ErKF ($Q_0$)	0.0119, 0.0335	0.0124, 0.0336	0.0129, 0.0337	0.0865	0.0887	0.0901
GP-ErKF (${\tilde{Q}}_0$) [9]	0.0075, 0.0194	0.0080, 0.0199	0.0084, 0.0202	0.0035	0.0054	0.0066
PreC-ErKF (${\tilde{Q}}_0$)	0.0055, 0.0145	0.0058, 0.0145	0.0075, 0.0153	0.0035	0.0076	0.0207
PostC-ErKF (${\tilde{Q}}_0$)	0.0049, 0.0138	0.0055, 0.0144	0.0059, 0.0146	0.0031	0.0050	0.0061

The bold numbers represent better results (the smaller number) and all values are rounded to four decimal digits.

). These findings suggest that the EPM outperforms the GPM. It is important to note that a comparison between the EPM and the GPM was not conducted in [9]; therefore, our results extend the work presented in that study.

As the initial error increases, the postC-ErKF shows significantly superior performance at some initial steps in comparison to the others. Surprisingly, the preC-ErKF reveals a counterintuitive performance compared to the traditional approach during this period. To be precise, in the case of $\beta =0.75$, Figure 2c illustrates that the preC-ErKF has the largest estimation error covariance among all candidates in both the angular position ${\widehat{\varphi }}_k$ and velocity ${\widehat{\Omega }}_k$ variables. This clearly indicates that during this time step, the pre-constrained strategy has low confidence in its estimation. As a consequence, the accuracy of the estimation will be diminished. However, after these early steps, the preC-ErKF quickly approaches the steady state, outperforming the unC-ErKF and reporting similar performance as the postC-ErKF.

The RMSEE and RMSCE mentioned earlier in this section are highlighted in Figure 3. This could serve as evidence that the results presented herein display a similar trend to that of Figure 2, owing to the correlation between the error covariance matrix and the filter’s accuracy. In particular, while the postC-ErKF demonstrates the most outstanding performance in both accuracy and robustness, the preC-ErKF outputs a significant overshoot when $\beta =0.5$ and $\beta =0.75$ compared to the unC-ErKF during the settling time (Figure 3b,c). Still, the pre-constrained approach outperforms the conventional ErKF in the steady state. Moreover, it can be clearly seen that the post-constrained filter exhibits superior efficiency and improvement compared to other competitors in the context of the RMSCE $C_{rms,k}$. This finding apparently supports our vision and discussion in Section 4.1.

Table 2 summarizes the time-averaged RMSEE and RMSCE of the second example for three $\beta$ configurations. It should be noted that, for consistency and in accordance with the same rationale, we also implement the constrained filters with ${\tilde{Q}}_0$ and the unconstrained filter with $Q_0$ as in the first example. This also applies to those results in Figure 2, Figure 3 and Figure 4. The table is revealing in several ways.

• Firstly, the postC-ErKF provides the smallest estimation error and constraint across all evaluated cases. For instance, in the worst case, when $\beta =0.75$, it gives ${\overline{E}}_{rms,k}=0.0059$ for $\widehat{\varphi }$, while the preC-ErKF, GP-ErKF, and unC-ErKF offer $0.0075$, $0.0084$, and $0.0129$, respectively. In other words, the post-constrained approach decreases the estimation error rate by approximately $21\%$, $30\%$, and $54\%$ compared to that of the pre-constrained, gain-constrained, and unconstrained methods, respectively. In the context of constraint error, similarly, it reduces the constraint error rate by approximately $70\%$, $7\%$, and $93\%$ compared to others. This table also supports our previous explanation that the GP-ErKF overly enforces the estimation. As a result, although the constraint error is small, the final estimation is poorer compared to the other methods.
• Secondly, the behavior of the preC-ErKF degrades much when faced with large initial errors. Specifically, as $\beta$ increases from $0.25$ to $0.75$, the pre-constrained strategy produces a corresponding increase in RMSCE with a significant gap value (e.g., from $0.0035$ to $0.0076$, and to $0.0207$, respectively). Conversely, the proposed post-constrained method yields a negligible small incremental gap in response to an increase in the initialization errors.

For a comprehensive analysis, we also compute the time-averaged RMSEE and RMSCE of each Monte Carlo run; in particular, their values can be computed as ${\overline{E}}_{rms,m}=\sqrt{{\displaystyle \frac{1}{K}}{\sum }_{k=1}^K{∥x_k^m-{\widehat{x}}_k^m∥}_2^2}$ and ${\overline{C}}_{rms,m}=\sqrt{{\displaystyle \frac{1}{K}}{\sum }_{k=1}^K{∥D_k^m{x}_k^m-{\widehat{D}}_k^m{\widehat{x}}_k^m∥}_2^2}$, respectively. The results with $\beta =0.5$ are illustrated in Figure 4. It is apparent from this figure that the postC-ErKF outclasses other filters in every Monte Carlo simulation for angular position estimation. Nevertheless, both the gain-constrained and pre-constrained methods provide smaller ${\overline{E}}_{rms,m}$ and ${\overline{C}}_{rms,m}$ compared to the unconstrained filter. These observations are in agreement with what we have discussed so far.

The numerical results thus far indicate that the PostC-ErKF outperforms the PreC-ErKF. Although Proposition 1 provides a special case in which both methods yield the same estimation results, the performance of the two approaches remains uncertain in other scenarios. However, based on the derivations presented, it can be suggested that the PostC-ErKF has an advantage because it linearizes the nonlinear constraints around the a posteriori estimate, resulting in linearized constraints that are closer to the true constraints. In contrast, the PreC-ErKF linearizes the constraints around the a priori estimate, which may lead to less accurate results.

6. Conclusions

In this paper, we have investigated two possible ways to incorporate linearized equality constraints using the EPM into the conventional ErKF framework that, to the best of our knowledge, has yet to be previously considered. Since the ErKF involves an extra error correction, the constraints can be enforced before or after this step; we refer to them as the preC-ErKF and postC-ErKF, respectively. Through a complete examination, we have mathematically proven that both proposed filters exhibit identical performance in all aspects for linear or particular nonlinear constraints that can be expressed as a certain form. Nevertheless, their responses if $g\left(x\right)$ does not satisfy the mentioned conditions are not mathematically established. However, the numerical Monte Carlo simulations have shown that the postC-ErKF provides a notable enhancement compared to the preC-ErKF in such scenarios. Specifically, the post-constrained filter exhibits smaller time-averaged estimation and constraint error and is significantly robust against the increasing initialization error and the constraint’s incompleteness.

It has been clearly shown that the ErKF’s correction step substantially impacts the estimation results when constraining nonlinear constraints are involved. Although this paper’s results deliver adequate evidence to determine whether the post-constrained filter should be prioritized over the pre-constrained approach, further rigorous studies about their behaviors in the presence of general nonlinear constraints are our motivation in future work.