Efficient Estimation of Sensor Biases for the 3-Dimensional Asynchronous Multi-Sensor System

An early work [4] for sensor registration dates back to the 1980s, where the authors identified various factors which dominate the alignment errors in the multi-sensor system and established a bias model to compensate the alignment errors. Under the assumption that there exists a bias-free sensor, the maximum likelihood (ML) estimation approach [4, 5, 6] and the least squares (LS) estimation approach [7] were proposed. However, the bias-free sensor assumption is not practical and a more common situation in practice is that all sensors contain biases. For the general situation where all sensors are biased, various approaches have been proposed in the last four decades. From the perspective of parameter estimation, these approaches can be divided into three types: LS estimation [8, 9], ML estimation [10, 11, 6], Bayesian estimation [12, 13, 14, 15, 16, 17, 18, 19, 20, 21]. Note that the sensor registration problem not only contains unknown sensor biases to be estimated, but also the target states, i.e., positions of the target at different time instances. The major differences among the above three types of approaches lie in the treatment for the unknown target states. For the LS approach, the target states are expressed as nonlinear functions of sensor biases in a common coordinate system, where the target states are eliminated and only sensor biases need to be estimated. As for the ML approach, a joint ML function for the sensor biases and the target states is formulated and maximized in an iterative manner (with two steps per iteration), i.e., one step to estimate the sensor biases and one step to estimate the target’s states. Instead of jointly estimating the sensor biases and the target states as in the ML approach, the (recursive) Bayesian approach exploits the Markov property of the transition of the target states between different time instances, i.e., the probability distribution of the target state at the next time instance only depends on the target state at the current time instance. The sensor biases and the target states are recursively estimated at every time instance.

Since sensor measurements are in its local polar coordinate system, the nonlinear coordinate transformation between the local polar and global Cartesian coordinates introduces an intrinsic nonlinearity in the sensor registration problem. The approaches mentioned above adopted different kinds of approximation schemes to deal with the nonlinearity, i.e., using linear expansions to approximate some nonlinear functions [8, 5, 9, 7, 4, 10, 12, 11, 6, 13, 14, 15, 16, 17] which usually leads to an uncontrolled model mismatch or using a large number of discrete samples (particles) to approximate some nonlinear functions which often requires an expensive computational cost when the number of sensors is large [18, 20, 21]. In addition to the schemes of dealing with the nonlinearity, many of the above works only consider the synchronous case, i.e., all sensors simultaneously measure the position of the target at the same time instance, which may not be always satisfied in many practical scenarios.

Recent works in [22, 23] proposed a semidefinite relaxation (SDR) based block coordinate descent (BCD) optimization approach to address the nonlinearity issue in the asynchronous sensor registration problem in the 2-dimensional scenario. The approach assumes the presence of one reference target moving at a nearly constant velocity (e.g., commercial planes and drones in the real world) and is effective in handling the nonlinearity while also guaranteeing an exact recovery of all sensor biases in the noiseless case. Consequently, it is interesting to explore such BCD algorithm in the 3-dimensional scenario, where more kinds of biases are involved and the estimation problem becomes even more highly nonlinear, i.e., more trigonometric functions with respect to different biases are multiplied together in the coordinate transformation. Furthermore, the BCD algorithm in [22, 23] needs to solve convex semidefinite programing (SDP) subproblems at each iteration, whose computational cost is $\mathcal{O}(M^{4.5})$ (where $M$ is the total number of sensors). Such a high computational complexity limits the practical use of the BCD algorithm though it can well deal with the nonlinearity issue. An interesting question is, instead of solving SDPs, whether there is an algorithm which has a lower computational complexity and can solve the original nonconvex problem with a guaranteed convergence. This paper provides a positive answer to the above question.

In this paper, we consider the 3-dimensional sensor registration problem, where more kinds of biases and noises are involved. Few works tackle the 3-dimensional registration problem except some notable works [12, 11, 7, 6, 1]. However, all of these works assume that sensors work synchronously. In this paper, we consider a practical scenario where all sensors are biased and work asynchronously. The major contributions are summarized as below:

The organization of this paper is as follows. In Section II, we introduce basic models of sensor measurements and the target motion. In Section III, we propose a nonlinear LS formulation for estimating all kinds of biases. To effectively solve the proposed nonlinear LS formulation, we develop a BCD algorithm in Section IV. Numerical simulation results are presented in Section V.

We adopt the following notations in this paper. Normal case letters, lower case letters in bold, and upper case letters in bold represent scalar numbers, vectors, and matrices, respectively. $\mathbf{X}^{T}$ and $\mathbf{X}^{-1}$ represent the transpose of matrix $\mathbf{X}$ and the inverse of invertible matrix $\mathbf{X}$, respectively. $[\bm{X}]_{i:j,m:n}$ represents the matrix formed by elements of matrix $\bm{X}$ from rows $i$ to $j$ and columns $m$ to $n$. $\mathbf{x}_{n}$ denotes the $n$-th component of $\mathbf{x}$ and $\|\mathbf{x}\|$ denotes the Euclidean norm of $\mathbf{x}$. $\mathbb{E}_{\mathbf{w}}\{\cdot\}$ is the expectation operator with respect to random variable $\mathbf{w}$. $\mathbb{R}^{M}$ represents the set of $M$-dimensional real vectors. Other notions will be explained when they appear for the first time.

By (8), we have the following convex quadratic reformulation for subproblem (10a):

where $\bm{H}_{v}\in\mathbb{R}^{6(K-1)\times 3K}$ is the coefficient matrix, $\bm{c}_{v}\in\mathbb{R}^{6(K-1)}$ is the constant vector, and $\bm{Q}\in\mathbb{S}_{+}^{6(K-1)}$ is a positive definite matrix. Detailed expressions of them are given in Appendix C-A of the Supplementary Material. Setting the gradient to be zero, the closed-form solution to subproblem (10a) is

where $\dagger$ denotes the pseudo-inverse operator.

The subproblem (10b) for updating $\Delta\bm{\rho}$ also has a closed-form solution since its objective is also convex quadratic with respect to $\Delta\bm{\rho}$. Subproblem (10b) can be reformulated as

where $\bm{H}_{\rho}\in\mathbb{R}^{6(K-1)\times M}$ is the coefficient matrix, and $\bm{c}_{\rho}\in\mathbb{R}^{6(K-1)}$ is the constant vector. Detailed expressions of them are given in Appendix C-B of the Supplementary Material. Similar to subproblem (10a), the closed-form solution of (10b) is

Since $g_{k}(\cdot)$ in (8) is linear with respect to the trigonometric functions of the angle biases, all the four subproblems (10c)-(10f) can be reformulated into a same QCQP form. For better presentation, we use $\Delta\bm{\vartheta}\in\Omega=\{\Delta\bm{\eta},\Delta\bm{\alpha},\Delta\bm{\beta},\Delta\bm{\gamma}\}$ to denote one kind of these angle biases and the corresponding subproblem can be reformulated as

where

and set $\mathcal{C}$ is defined as

In (15), $\bm{H}_{\vartheta}\in\mathbb{R}^{6(K-1)\times 2M}$ is the coefficient matrix and $\bm{c}_{\vartheta}\in\mathbb{R}^{6(K-1)}$ is the constant vector. Detailed expressions of them are given in Appendix C-C of the Supplementary Material.

Problem (15) is a non-convex problem due to its non-convex equality constraints. In general, such class of problem is known to be NP-hard [28] and the SDR based approach [29, 30] was used to solve it. In particular, in [23], the authors utilized the SDR technique to solve a specific case of problem (15) where it can be reformulated as a complex QCQP, and proved that the SDR is tight, i.e., the global minimizer of the QCQP problem can be obtained by solving the SDR under a mild condition. This motivates us to ask whether problem (15) can also be globally solved and whether there are some efficient algorithms better than solving the SDR.

Note that the non-convex constraints in problem (15) enjoy an “easy-projection” property. In particular, let $\textrm{Proj}_{\mathcal{C}}(\bm{x})$ denote the projection of any non-zero point $\bm{x}\in\mathbb{R}^{2M}$ onto $\mathcal{C}$. Then $\textrm{Proj}_{\mathcal{C}}(\bm{x})$ admits the following closed-form solution:

where $\bm{D}_{\bm{x}}$ is a diagonal matrix defined as

and $\otimes$ denotes the Kronecker product. Such “easy-projection” property together with recent results of the ADMM for nonconvex problems [31] provides us a potential way of efficiently solving problem (15) with a guaranteed convergence. The choice of the ADMM algorithm over other potential methods such as the gradient projection (GP) method is discussed in Remark 1. In the next, we will apply the ADMM for solving it and also show that the ADMM is able to globally solve problem (15) under mild conditions.

Based on such “easy-projection” property, we introduce an auxiliary variable $\bm{x}=\bm{z}$ to split problem (15), which makes all subproblems in the ADMM iterations admit closed-form solutions. In particular, subproblem (15) is reformulated as

where $\iota_{\mathcal{C}}(\bm{z})$ is the indicator function for set $\mathcal{C}$, given as

The augmented Lagrangian function for problem (16) is

where $\rho>0$ is the penalty parameter and $\bm{\lambda}\in\mathbb{R}^{2M}$ is the Lagrange multiplier for the equality constraint $\bm{x}=\bm{z}$. Then, the ADMM updates at iteration $\ell$ are

Note that subproblem (19a) is an unconstrained strongly convex quadratic problem and subproblem (19b) is actually a projection problem, both of which can be solved in closed forms. Details are given in Algorithm 1.

Next, we discuss the convergence property of Algorithm 1. Before doing this, we first study the structures of coefficients $\bm{H}_{\vartheta}$ and $\bm{c}_{\vartheta}$ in problem (15). Note that $\bm{H}_{\vartheta}$ and $\bm{c}_{\vartheta}$ can be viewed as random matrices/vectors and the randomness is caused by those zero-mean Gaussian noises, i.e., $\bm{w}_{k}$ in (3) and $\bm{n}_{k},\,\dot{\bm{n}}_{k}$ in (5). To understand the effects of these noises, let us suppose that there is no noise, i.e., $\bm{w}_{k}=\bm{0},\bm{n}_{k}=\dot{\bm{n}}_{k}=\bm{0}$, for all $k$, and all the other biases $\Omega\setminus\Delta\bm{\vartheta}$ and velocity $\bm{v}$ take the true values. Then by the same derivations in Appendix C of the Supplementary Material (with compensation factors $\lambda_{\eta}=\lambda_{\phi}=1$), we attain new $\bar{\bm{H}}_{\vartheta}$ and $\bar{\bm{c}}_{\vartheta}$. Intuitively, $\bar{\bm{H}}_{\vartheta}$ and $\bar{\bm{c}}_{\vartheta}$ represent the “true” parts of $\bm{H}_{\vartheta}$ and $\bm{c}_{\vartheta}$ by removing measurement noise $\bm{w}_{k}$, motion process noises $\bm{n}_{k}$ and $\dot{\bm{n}}_{k}$, and errors of other biases and velocity. Consequentially, the “errors” in $\bm{H}_{\vartheta}$ and $\bm{c}_{\vartheta}$ can be defined as

Technically, decomposing $\bm{H}_{\vartheta}$ and $\bm{c}_{\vartheta}$ in (20) provides a natural way of characterizing the convergence behavior of Algorithm 1, given in Theorem 1.

The first statement of Theorem 1 is a direct result of [31, Corollary 2], which ensures the convergence of the ADMM for a class of the nonconvex problem (with a Lipschitiz continuous gradient) over a compact manifold. The second statement of Theorem  1 on the uniqueness of the stationary point is due to the special structure of problem (16). The entire sequence convergence in Theorem 1 is a combined result of the general convergence of the ADMM in [31, Corollary 2] and the special structure of problem (16).

The proposed BCD algorithm for solving problem (8) is summarized as Algorithm 2. Three remarks on its convergence, computational complexity, and comparison with other BCD variants are given in order.

Convergence of Algorithm 2. The well-known convergence analysis of BCD [33, 34] shows that if all subproblems in the proposed BCD Algorithm 2 have unique solutions and can be globally solved, then the iterates can converge to a stationary point of the non-convex problem (8). The uniqueness and the global optimality of solutions $\bm{v}^{*}$ (in (12)) for subproblem (10a) and $\Delta\bm{\rho}^{*}$ (in (14)) for subproblem (10b) can be guaranteed if $\bm{H}_{v}$ and $\bm{H}_{\rho}$ are of full rank. For subproblems (10c)–(10f), the full rankness of $\bm{H}_{\vartheta}$ together with sufficiently small $\Delta\bm{H}_{\vartheta}$ and $\Delta\bm{c}_{\vartheta}$ in (20) further ensure that each of the corresponding subproblems has a unique solution and can be numerically attained by the proposed ADMM Algorithm 1 (cf. Theorem 1). According to the definitions of $\bm{H}_{v}$, $\bm{H}_{\rho}$, and $\bm{H}_{\vartheta}$ in Appendix C of the Supplementary Material and Assumption 1, we know that (i) $\bm{H}_{v}$ in (11) is always of full rank since each sensor has at least one measurement; (ii) $\bm{H}_{\rho}$ in (13) and $\bm{H}_{\vartheta}$ in (15) are of full rank with probability one (whose proof is provided in Appendix D of the Supplementary Material). Based on the above discussion, we can conclude that Algorithm 2 can converge to a stationary point of the non-convex problem (8) if those $\Delta\bm{H}_{\vartheta}$ and $\Delta\bm{c}_{\vartheta}$ along the BCD iterates are all sufficiently small. Finally, we remark that this sufficient small condition provides a theoretical understanding for the convergence of Algorithm 2 and in general it depends on the initial point. Our numerical simulation results in Section V suggest that such a condition always holds in various scenarios by simply initializing all biases to be zeros. The results in the noiseless setting (Section V-A) even verify the global optimality of Algorithm 2.

Computational complexity of Algorithm 2. All subproblems in the proposed BCD updates (10a)–(10f) take two special forms, i.e., unconstrained linear LS problems (i.e., subproblems (11) and (13)) and quadratic programs with special non-convex constraints (i.e., subproblem (15)). It can be shown that the computational complexity of solving subproblem (11), (13), and (15) is $\mathcal{O}(K^{2}+K)$, $\mathcal{O}(M^{2}+MK+M)$, and $\mathcal{O}\left(I\left((2M)^{2}+2MK+2M\right)\right)$ (where $I$ is the total iteration number of the ADMM), respectively. It is worthwhile mentioning that the computational complexity of solving (15) by using the ADMM, which is $\mathcal{O}(I(2M)^{2}),$ is generally much less than that of using the SDR technique, which is $\mathcal{O}((2M)^{4.5})$ [29]. The computational time comparison in Table II (Section V-B2) demonstrates the much better computational efficiency of using the ADMM over the SDR based approach for solving subproblem (15).

BCD variants. Finally, we remark that there are several nice extensions of the BCD framework such as the block successive minimization method [35] and the maximum block improvement (MBI) method [36]. Both of them can be applied to solve problem (8) with guaranteed convergence (with a suitable approximation of the objective function). The sequential optimization version of MBI [36] can effectively handle nonconvex constraints. In this paper, we still use the basic version of BCD because the nonconvex BCD subproblems with respect to angle biases enjoy the unique solution property (cf. Theorem 1), which reveals an interesting insight into the considered bias estimation problem that the true bias can be recovered under the ideal case (i.e., $\Delta\bm{H}_{\vartheta}=\bm{0}$ and $\Delta\bm{c}_{\vartheta}=\bm{0}$). MBI also enjoys the same property of the uniqueness of the subproblems but it requires more computational cost than classical BCD. A parallel implementation of MBI can improve its computational efficiency but this is beyond the scope of this paper. Simulation results in Section V-A further show that all biases are exactly recovered by our proposed BCD Algorithm 2 in the noiseless case.

In this subsection, we test the convergence behavior of the proposed BCD for solving problem  (8) with $\bm{Q}_{k}=\bm{I}$, $\forall k$. We consider the noiseless case with the measurement noise variance $\sigma_{\rho}^{2}=0\,\textrm{km}$, $\sigma_{\eta}^{2}=\sigma_{\phi}^{2}=0^{\circ}$ and the target motion noise $q=0\,\textrm{m}^{2}/\textrm{s}^{3}$. In such a case, the true biases achieve a zero loss and hence they are a global solution of problem (8). Fig. 3 plots the optimization residuals over the number of iterations and the elapsed time. The optimization residuals for different kinds of biases are defined as the sum of the squared estimation error over all sensors, e.g., the optimization residual for the range bias is $\sum_{m=1}^{M}(\Delta\hat{\rho}_{m}-\Delta\rho_{m})^{2}$, where $\Delta\hat{\rho}_{m}$ is the estimated range bias of sensor $m$. The convergence curves in Fig. 3 demonstrate the global optimality of the proposed BCD algorithm in the noiseless case, i.e., the loss function converges to zero and the finally returned solution by the BCD algorithm is very close to the true biases. The similar convergence behavior has also been observed in many other scenarios. In particular, Fig. 4 shows the convergence curves of the proposed BCD algorithm in $20$ randomly generated scenarios²²2 The sensors’ positions are uniformly placed over the region $[-20,20]^{2}\times[-1,1]\,$km and sensors’ biases are generated from the Gaussian distribution, i.e., $\Delta\rho\sim\mathcal{N}(0,(1\,\textrm{km})^{2})$, and $\Delta\eta,\Delta\alpha,\Delta\beta,\Delta\gamma\sim\mathcal{N}(0,(3^{\circ})^{2})$..

In this subsection, we evaluate the estimation performance of the proposed BCD algorithm in the noisy case. Note that the global optimality in the noisy case is generally hard to examine, as the global solution of the corresponding problem is unknown. However, our numerical results show that the solution found by the BCD algorithm achieves a pretty satisfactory estimation performance.

In the rest of simulations, we use the root mean square error (RMSE) as the estimation performance metric. The root of hybrid Cramer-Rao lower bound (RHCRLB) [38, 7] is used as the performance benchmark. The RMSEs and RHCRLBs in all of the following figures are obtained by averaging 100 independent Monte Carlo runs. We will compare the proposed BCD algorithm with the aforementioned three approaches under different setups.