An Iterative Extended Kalman Filter for Coherent Measurements of Incoherent Network Nodes in Positioning Systems

Many positioning and tracking applications use spatially distributed sensor stations, each equipped with coherent measurement channels. The coherent data set of each node is incoherently measured to the data sets of all other nodes, avoiding expensive synchronization procedures between the stations. Usually, the measurements are evaluated by mapping the incoherently measured complex valued data on real valued data like angle of arrival or received signal strength. After this preprocessing step, recursive filters fuse the real valued data to estimate a system state. Unfortunately, even though the original measurements are performed in additive white Gaussian noise environments, the preprocessing step can result in correlated, noise shaped errors, whose variance is system state dependent. Hence, the additive white Gaussian noise assumption, which is commonly drawn in Kalman filters, is violated. Therefore, this paper proposes an iterative extended Kalman filter, which estimates the state of a nonlinear real valued system via a complex valued measurement model that consists of incoherent sensor stations, each with several coherent measurement channels. Since the proposed iterative extended Kalman filter is well suited for distributed positioning systems with several stations, a transmitter is localized in a simulation via two sensor stations, each measuring the received signal’s amplitude and phase at four channels. To illustrate the advantages of the proposed algorithm, the direct measurement evaluation using the proposed algorithm is compared to a Kalman filter that evaluates the received signal strengths and angle of arrivals at each sensor station. Finally, a reflecting wall is incorporated into the simulation scenario to demonstrate the flexibility of the proposed Kalman filter.


I. INTRODUCTION
In many applications, for example wireless positioning [1], [2], noise source and jammer detection [3], and imaging [4], receive signals are often evaluated using spatially distributed sensors, in which each sensor might be equipped with several coherent receive channels, each yielding complex valued measurement data. Hence, the relation between the phases of the measured complex valued data can be evaluated. a better computational efficiency than common solutions. Thus, for the exemplary source localization problem in Fig. 1, the proposed Kalman filter offers a straightforward procedure implicitly using the RSS and the incoherently measured phases.
The remainder of the paper is organized as follows. First, a mathematical description of the assumed system and measurement model is shown in Section II. Next, Section III describes the IEKF derivation and the resulting algorithm. To validate the algorithm, a transmitter is located by evaluating the receive signal's amplitudes and phases in Section IV. The algorithm is compared to an AOA/RSS based EKF, whose structure and noise shaping property is examined in detail. Furthermore, the algorithms are applied to a scenario with reflections at a wall. Section V gives the conclusion.
Notation: An underscore denotes a complex number, vectors are denoted with an arrow over the variable, matrices are shown in bold letters, (·) T is the transpose, (·) H is the conjugate transpose of a matrix, I n is the n×n identity matrix, 0 m×n is the m × n zero matrix, * denotes the conjugate of a complex number, | · | is the absolute value of a complex number, · 2 is the Euclidean norm, a hat over the variable denotes an estimated value, and H J denotes the Hessian of scalar function J .

II. SYSTEM-AND MEASUREMENT MODEL
The system model describes the time dependent system state propagation. In this work, a nonlinear discrete system model of the form VOLUME 8, 2020 where k is the time index, x k ∈ R m is the system state, u k ∈ R n is the control input, w k ∼ N o ( 0, Q k ) is the zero-mean white Gaussian noise with Q k as the covariance matrix, f k : R m × R n → R m , and G k ∈ R m×o , will be involved in the prediction step of the proposed Kalman Filter.
Measurements are taken by several measurement groups, each with several coherent channels. On the one hand, the measurement groups are sufficiently time-synchronized to assume a constant system state for all sensor stations during the measurement at time step k. On the other hand, the synchronization is not sufficient to assume constant phase relations between the sensor stations. This is modeled by the random unknown phase shift α η,k at time instance k and yields complex valued measurements from a real valued system model as where η ∈ {1, . . . , N m } is the group's index, N m is the number of incoherent measurement groups, q η is the number of coherent channels of the ηth group, z k ∈ C q is a vector containing all measurement values, q = η q η , z η,k ∈ C q η is a vector containing the measurement values of the ηth group, is the uniformly distributed random phase shift, and v η,k ∼ CN q η ( 0, C η,k ) is complex valued zero-mean white Gaussian noise, with C η,k as the covariance matrix. Fig. 1 illustrates an exemplary measurement model, in which a signal is emitted by a transmitter and received by several sensors that evaluate the signal's phase and amplitude.

III. DERIVATION OF THE ALGORITHM
The Kalman filter consists of two steps, namely, the prediction and the update. In principle, the system state x k is considered as a random variable, whose probability density function (PDF) is assumed to be Gaussian and calculated by the two steps. This is shown subsequently.

A. PREDICTION STEP
The prediction step estimates the next system stateˆ x k|k−1 and state covariance matrix P xx,k|k−1 solely depending on the prior system stateˆ x k−1|k−1 and prior state covariance matrix P xx,k−1|k−1 by propagating the system state using the nonlinear discrete system model (1). For this purpose, it calculates the PDF p( x k | z 1:k−1 , u 1:k−1 ) ≈ N m (ˆ x k|k−1 , P xx,k|k−1 ), which is only approximately normally distributed, because of the system model's nonlinearity.
At first, the first-order Taylor series expansion of f k−1 ( x k−1 , u k−1 ) is calculated at the current estimation x k−1|k−1 and the control input u k−1 as The state vector estimation can be calculated by the expectation operator yieldinĝ where the first-order Taylor series expansion (3) is used. Similarly, the covariance matrix is calculated as Note that the results are well known in literature [32], but derived in a compact manner.

B. UPDATE STEP 1) MAXIMUM A POSTERIORI ESTIMATION
Now, the update step merges the measurement z k with the predicted stateˆ x k|k−1 and state covariance matrix P xx,k|k−1 , yielding an improved state estimationˆ x k|k and state covariance matrix P xx,k|k . In this process, the system state x k is estimated together with the unknown phase shifts α k = . . . , α η,k , . . . T , resulting in the posterior joint probability density function p( x k , α k | z 1:k , u 1:k−1 ), which is calculated by the proposed update step. The derivation starts using Bayes' rule [32] p( x k , α k | z 1:k , u 1:k−1 ) = p( z k | x k , α k , z 1:k−1 , u 1:k−1 ) · p( x k , α k | z 1:k−1 , u 1:k−1 ) where at (a) the independence between z k , z 1:k−1 , and u 1:k−1 because of the Markov assumption [32], the independence between x k and α k , and the independence between α k , z 1:k−1 , and u 1: , and the statistical independence of the measurement groups are considered. Since both the predict and the update step use linearized models to estimate the system state and the covariance, at (c) the approximation of p( x k | z 1:k−1 , u 1:k−1 ) ≈ N m (ˆ x k|k−1 , P xx,k|k−1 ) is used, as in [32]. The new system state can be calculated using the maximum a posteriori (MAP) estimation [33] aŝ with the measurement function h xα,η,k ( x k , α η,k ) = h η,k ( x k )e jα η,k , and J as the objective function to be minimized. Here, the different measurement groups were merged by The resulting optimization problem (7) is non-convex and hence, we propose to initially estimate the phases at the predicted system state and then iteratively update both the system state x k and the unknown phases α k .

2) INITIAL PHASE ESTIMATION
To ensure reliable convergence toward the global minimum of the optimization problem (7), the values of x k and α k have to be initialized close to the optimum. Therefore, the predicted state vectorˆ x k|k−1 and their corresponding optimal phaseŝ α k|k−1 offer a good initialization. To determine the phaseŝ α k|k−1 , (7) has to be minimized, yieldinĝ and thereforê as initial value for the phase shifts. The solution of the optimization problem (11) is shown in Appendix A.

3) ITERATION STEP
In the ith iteration step, the first-order Taylor series expansion of the measurement function h xα,k ( x k , α k ) is used on the last estimationsˆ x i k andˆ α i k as with the differences x i k = x k −ˆ x i k and α i k = α k −ˆ α i k , and the Jacobians Approximating J in (7) using the first order Taylor expansion in (12) yields the quadradic function J i of the ith iteration step as with where (16) follows after a few algebraic manipulations, which are shown in Appendix B. Terms, which are independent of x k and α k are summarized in const.. Furthermore, the inverse covariance matrix was used. Here, the precision matrix P −1 αα,k|k−1 reflects the knowledge about the phases α k . Hence, P −1 αα,k|k−1 = 0 N m ×N m holds, because no a priori information about the incoherent phases α k exist. Thus, P xα,xα,k|k−1 does not exist and consequently, the following calculations are performed in terms of P −1 xα,xα,k|k−1 instead of P xα,xα,k|k−1 as in other Kalman filter derivations [16], [32], [34]. Now, one Gauss-Newton step is used for minimizing (16), as in [21], [35]. With the gradient of J i and the Hessian of J i the Gauss-Newton iteration becomes [35] ˆ with as the Kalman gain. At (d), is used, which can be proven by inserting the Kalman gain (24) in (25). The covariance of the ith iteration can be calculated as the inverse of the Hessian [32], yielding and has to be evaluated after the last iteration to obtain the covariance matrix for the next Kalman step.

ALGORITHM SUMMARY AND INTERPRETATION
Performing the steps in III-B.3 N i times in an iterative manner yields the proposed ICIEKF, which is shown in Algorithm 1. First, the new system state is predicted in (27). Second, the unknown phases are initially estimated using the predicted system state in (28). Afterwards, the predicted state and the corresponding phases are used to start N i iterations of the proposed update step in (29). For N i = 1, an EKF-like algorithm is created. Finally, the state covariance for the next Kalman step is updated using the final state estimationˆ x N i k in (30). The resulting algorithm can be intuitively explained by comparing the proposed ICIEKF with the standard IEKF [21]. Similarly to [21], the ICIEKF performs an iterative maximum likelihood search for the next system state, but simultaneously estimates the unknown measurement phases. The IEKF in [21] is solving the non-convexity issue by recursion, whereas the ICIEKF needs an initial phase estimation (28a).
In the standard IEKF, the Kalman gain is responsible for both inverting the linear measurement equations, such that the state update can be evaluated from the difference between real and hypothetical measurements, and balancing between the uncertainty of the measurements and the predicted state. In the ICIEKF, the Kalman gain in (29f) is also implicitly incorporating possible phase estimation adjustments such that the measurements can be better explained. This can be seen by examining P −1 xα,xα,k|k−1 , incorporating both the prior information of the system state P −1 xx,k|k−1 and the precision matrix of the phases without prior information P −1 αα,k|k−1 = 0 N m ×N m , and H i xα,k , incorporating both adjustments of the estimated system state H i x,k and the estimated incoherent phases H i α,k . Hence, the corresponding phase estimation adjustments can be calculated in (29g) by solely evaluating the difference between the real and hypothetical measurements.

IV. EVALUATION OF THE PROPOSED ALGORITHM
As an example, the proposed algorithm is used to localize a transmitter in 2D using multiple antenna arrays and is compared to an EKF from [15], that evaluates the angle of arrival (AOA) and receive signal strength (RSS) at each array, which are estimated in a preprocessing step. In the following, the transmitter (TX) is localized using two spatially distributed linear antenna arrays, each consisting of four coherent receive antennas measuring the signal's amplitude and phase. The transmit and receive antennas are assumed to be ideal, with isotropic radiation patterns. Using the notation, introduced in Section II, η = {1, . . . , N m } is the array index, and ξ = {1, . . . , q η } is the antenna index. Furthermore, the subscript n indicates the description in the navigation frame, and b η the description in the body frame of the ηth array. Each antenna is assumed to be disturbed by uncorrelated additive white Gaussian noise with the same variance, yielding the covariance matrix as C η,k = I q η σ 2 m , with σ m as the standard deviation, which is equal for all groups.
Note that the following simulations are only intended to provide an illustrative example, showing the benefit of the proposed algorithm. They are not intended to perform a quantitative algorithm evaluation, for several reasons. First, the shown localization results are only the implementation of the ICIEKF for the exemplary measurement model, but the proposed algorithm has many different applications. Second, the algorithm is compared to a AOA/RSS based EKF, which allows for a very illustrative comparison showing the disadvantages of preprocessing, but is not necessarily the best competitor. Third, the performance is strongly depending on the simulation's parameters as array size, noise, and movement model, and hence, a fair quantitative comparison is impossible.

A. SYSTEM MODEL
In the subsequent simulations, the well established constant velocity model [33] is used, which recursively estimates the state vector with r TX,n,k as the transmitter position, and v TX,n,k as the transmitter velocity in coordinates of the navigation frame n, respectively. It is assumed, that the transmitter constantly moves, yielding the system model as with T s as the sample time, as the covariance matrix of w k , with σ a as the standard deviation of the acceleration noise error, as in [33]. Note that G k = I 4 holds.

B. MEASUREMENT MODEL
Assuming the transmitter to emit spherical waves [36] and being located at position r TX,n,k , the complex valued signal s RX at a receiver at an arbitrary position r P,n results in s RX ( r P,n , r TX,n,k ) = A r P,n − r TX,n,k 2 e −jk c r P,n − r TX,n,k 2 , with A in a.u. m as the transmitter's amplitude, a.u. as arbitrary unit, k c = 2π f c c 0 as the wave number, f c as the carrier frequency, and c 0 as the speed of light. Note that the unknown transmitter signal phases are incorporated in the unknown phase shifts α η,k .
Hence, the measurement model of the ηth array is stated as s RX ( r (η.ξ ),n , r TX,n,k ) . . .
where r (η.ξ ),n denotes the position of the ηth array's ξ th antenna in navigation coordinates. Then, the measurements are simulated using (2).

C. AOA/RSS EKF
Complex values are measured at each antenna. In [15], an EKF is described, which uses AOA and RSS for system state estimation. Thus, a mapping from the complex valued measurements to AOA and RSS is needed and described in this section. Furthermore, the used AOA/RSS EKF involves the system model (32) and the measurement model, which is also presented in this section.

1) AOA AND RSS ESTIMATION
There are many ways to estimate the AOA using phase measurements at spatially distributed antennas [25]. Here, the AOA is estimated by a delay-and-sum beamformer. In doing so, the incident wave is implicitly assumed plane and equally strong at each antenna. Thus, the AOA estimation is impaired for close transmitter positions and large antenna arrays.   2 shows the geometry of a linear array with equidistantly distributed antennas with distance d η . Assuming a planar wave, the phase shift of the receive signal of the ξ th antenna is k c (ξ − 1) l η,k , with l η,k = d η sin θ η,k , and θ η,k as the assumed AOA. Hence, the steering vector [25] inverts the phase shift for the correct AOA assumption. Therefore, the AOA is estimated by maximizing the power with [25]θ The received signal strength (RSS) at one antenna array is calculated as the absolute values' mean of the complex valued antenna measurements z η,ξ,k , resulting in The AOA and RSS values are stacked, yielding the real valued measurement vector

2) AOA/RSS EKF MEASUREMENT MODEL
The vector, pointing in the ηth array's body frame b η from the origin to the transmitter, can be calculated as with R b η n denoting the rotation matrix from the navigation frame n to the array's body frame b η , r TX,n,k denoting the transmitter's position vector in navigation coordinates, and r η,n denoting the position vector of the ηth array in navigation coordinates. Then, the angle of arrival can be computed from the transmitter's position as shown in Fig. 2 by The received signal strength can be determined from the transmitter's position as A η,k = |s RX ( r η,n , r TX,n,k )| = A r η,n − r TX,n,k 2 .
Combining AOA and RSS yields the measurement function Then, the EKF evaluates the AOA and RSS at every array as where z AOA/RSS k ∈ R 2N m denotes the measurement vector, and v AOA/RSS k ∼ N 2N m ( 0, R AOA/RSS k ) is uncorrelated zero-mean white Gaussian noise, which is commonly assumed [15].

3) AOA AND RSS MEASUREMENT NOISE
In this section, the noise behavior of the AOA and RSS estimations, which are assumed to be zero-mean white Gaussian noise for the EKF, are studied. For this purpose, a linear antenna array with q η = 4, d η = λ c 2 , a transmitter amplitude of A = 1 a.u. m , and a system frequency of f c = 1GHz is assumed. The antennas are centered symmetrically around the origin of the array's frame b η in x b η -direction, see Fig. 2. Now, a transmitter is located at r ηTX,b η ,k = 0 10m T and hence, the correct AOA is 0 • . Performing 10 000 AOA estimations with σ m = 0.1a.u. yields the AOA histogram in Fig. 3. It can be seen that side lobes arise at ±45 • , which dominate the variance and yield a significantly broadened normal distribution with the same variance. Note that the noise shaping effect depends on the noise level, the array geometry, and the transmitter's position. In Fig. 4, the standard deviation of the AOA estimation σθ η,k is calculated for different transmitter positions r ηTX,b η ,k = r ηTX,x,b η ,k r ηTX,y,b η ,k T .
The positions of the four antennas are marked by red crosses. Each AOA standard deviation σθ η,k was calculated by 10 000 simulation cycles per point. Fig. 4 shows, that the standard deviation of the AOA estimation depends strongly on the AOA itself and the distance between the array and the transmitter.  Inserting a noisy signal in (38) to estimate the RSS yieldŝ where v η,ξ,k = v η,ξ,k e −j(arg{h η,ξ,k }+α η,k ) ∼ CN (0, σ 2 m ) holds, as complex valued zero-mean Gaussian white noise is phase-shift invariant [37]. Since each summand is the absolute value of a complex random variable with non-zero mean, the signal strength at each antenna is Rician distributed [38]. Thereby, the Rice distribution's mean is slightly higher than the correct signal strength |h η,ξ,k |. Because of the following summation of the signal strengths and the independence of the noise terms,Â η,k is approximately normally distributed [35]. To investigate the behavior of the originated error offset,   ξ =1 |h η,ξ,k | as the true value, and Fig. 6 the dependence of the RSS value's standard deviation σÂ η,k on different noise levels, respectively. The transmitter is located at r ηTX,b η ,k = 0 r ηTX,y,b η ,k T , whereby r ηTX,y,b η ,k varies from 0 to 10m in both cases. The values were calculated with 10 000 simulation cycles per point. The error becomes stronger for lower SNR values. Furthermore, the RSS's standard deviation σÂ η,k is distance dependent for high noise levels and distance independent for low noise levels.
In summary, the choice of the AOA and RSS variance is challenging, because the current noise level depends on the system state. To enable an as fair as possible algorithm comparison in the following section, the AOA and RSS variance are determined by first simulating trajectories, second estimating the AOA and RSS, and then estimating their variance for each trajectory by comparison with the correct RSS and AOA.

D. SIMULATIVE COMPARISON OF THE ALGORITHMS
In this section, a transmitter is located in the x n y n -plane using the ICIEKF and the AOA/RSS EKF in two different scenarios.  withˆ r TX,j,k as the estimated position, and r TX,ref,j,k as the true position, the total RMSE is given by The initial value of the state vector for trajectory generation and estimation is  In Fig. 7(a) the simulated scenario is shown, where the random trajectories are gray solid and the antennas are marked as black crosses. Furthermore, for a part of one trajectory the localization results are shown with high resolution for two noise levels in Fig. 7(b). Then, the measurement values, which are calculated by (2) and (34), are used to estimate the transponder's position using the ICIEKF and AOA/RSS EKF. Fig. 8 shows the resulting RMSEs depending on the noise level. Note, that besides the AOA/RSS EKF and ICIEKF, a pure AOA EKF, a pure RSS EKF, and a fully coherent IEKF were simulated. The pure AOA EKF is realized by omitting the RSS measurements, and the pure RSS EKF by omitting the AOA measurements. The IEKF for a fully coherent system is easily implemented by evaluating the real and imaginary part of the measurement model (34), like in the ICIEKF. Fig. 8 shows, that the RSS EKF yields the worst RMSE for high noise, because of the unfavorable RSS offset, while the AOA EKF yields the worst RMSE for low noise. The AOA/RSS EKF implicitly uses the combined information and hence outperforms both the RSS and AOA EKF. It especially shows a RMSE benefit, when the RSS and AOA EKF yield approximately the same result at σ m = 10 −2.5 a.u.. Generally, the performance of all RSS and AOA based algorithms are limited for low noise, because the model errors dominate the noise errors. Hence, the ICIEKF can outperform the RSS and AOA based algorithms especially for low noise conditions. To illustrate that behavior, the localization results of an exemplary part of a trajectory is shown in Fig. 7(b), where the ICIEKF becomes arbitrarily exact for low noise, while the AOA/RSS EKF cannot improve its accuracy. For high noise conditions, the ICIEKF is performing slightly better than the RSS and AOA based algorithms. Assuming the measurement model to be coherent, the localization RMSE can be further improved, because the known phase provides further information. For low noise, the coherent IEKF and the ICIEKF show the same convergence behavior. Note, that both the coherent IEKF and the ICIEKF suffer from the nonlinear behavior of the RSS, when the RMSE exceeds 10 −5 m.

2) SIMULATION SCENARIO 2
To demonstrate the flexibility of the general ICIEKF formulas in Algorithm 1, a reflecting wall located at y n = r mirror,y,n = −4m and pointing in x n -direction is incorporated in the second simulation scenario, see Fig. 9(a). The reflection is modeled by mirroring the transmitter at the wall, yielding a second transmitter with the same amplitude at the wall's back side [39]. Therefore, the measurement model of the ηth array is extended to s RX ( r (η.ξ ),n , r TX,n,k ) + s RX ( r (η.ξ ),n , r mirror TX,n,k ) . . .
with the mirrored transmitter position r mirror TX,n,k = r TX,x,n,k 2r mirror,y,n − r TX,y,n,k .
(50) Fig. 10 shows the resulting RMSE errors. Since, the measurement model can be adapted, the coherent IEKF and ICIEKF can easily localize the transmitter. The RMSE is a bit lower than in the first scenario, because the virtual second transmitter increases the signal level. Since the ICIEKF without mirror model and the RSS/AOA based Kalman filters are impaired by severe model errors, their performance is heavily degraded. Generally, the beacon is shifted away from the mirror, because the RSS is increasing.

V. CONCLUSION
In this paper, a novel iterative extended Kalman filter for nonlinear real valued system models and incoherent complex valued measurement groups was derived. To illustrate its superiority, a transmitter was localized by both the proposed iterative extended Kalman filter and a Kalman filter that evaluates the AOA and RSS, demonstrating the drawbacks of preprocessing incoherent measurements. Though the proposed iterative extended Kalman filter is especially suited for various localization environments with distributed sensors as frequency modulated continuous wave radars, multi frequency radars, or magnetic positioning systems, the concept is applicable for arbitrary recursive estimation problems. Further work remains, for example, in extending the algorithm to support the estimation of several systems states, yielding incoherently superposing signals. This enables tasks as the localization of several incoherent transmitters.