Position Estimation

At first it is assumed that the object can be considered a point as depicted in Figure 5.1a on the next page. In this case, the state x^T,^{htTt˙Tiof the} object consists of its position t =£x y z¤^Tand velocity ˙t =£ ˙x ˙y ˙z¤^T.

Dynamic and Measurement Model

The dynamic behavior—the motion—of the object is described by means of the linear discrete-time dynamic system

x_k+1= A · x_k+ w_k , (5.1)

164 5 Applications

Figure 5.1: Examples for range-based localization problems. Based on the measured ranges between landmarks (LMs) at known positions and the unknown position of the object, the object’s trajectory has to be estimated.

In case of pose estimation (b), the object possesses itself several landmarks (gray circles) to which range measurements can be performed. The center of mass is indicated by the cross.

where the noise w_k is assumed to be zero-mean white Gaussian. For a position velocity model [207], the matrix A and the covariance of the process noise C^ware given by

A =· I T · I

corresponds to the process noise covariance of the continuous time sys-tem model withσ²_c,ξbeing the variances of dimensionξ ∈ {x,y,z}.

Range measurements to N landmarks at the known positions S_i∈R^3with i = 1,...,N are incorporated. The nonlinear relation between the object position and the landmark position is given by

ρ_k,i=°

°S_i− t_k°

°₂ , (5.3)

whereρk,i is the Euclidean distance between object and landmark.

5.1 Range-based Localization 165

In a real scenario, the ranges cannot be measured exactly, i.e., measure-ment uncertainty has to be considered, which is usually done by incorpo-rating a noise process into (5.3). Two possibilities arise for incorporation.

In the first case, which is the standard model ρk,i=°

°S_i− t_k°

°₂+ v_k,i, (5.4)

the noise process vk,idirectly affects the range rk,i. In the second case ρk,i=

°

°

°Si− t_k− v_k,i

°

°

°₂ , (5.5)

which is called noise before non-linearity [44], the noise process affects the difference between object and landmark position. This measurement model can be interpreted such that the positions of the landmarks are uncertain. In both measurement models, the noise process is assumed to be zero-mean white Gaussian. In the following, the second model (5.5) is considered for mainly two reasons: First, the standard model (5.4) is only appropriate in situations where the distanceρk,iis large compared to the variance of the noise vk,i. Otherwise, negative ranges are possible, which is not true in reality. This problem cannot occur in the second measurement model. Second, the model in (5.5) allows analytic moment matching.

Analytic Moment Matching

Thanks to the linearity of the dynamic model (5.2), the standard Kalman filter prediction step (2.12) can be employed for propagating the state from time step k to time step k + 1. To obtain also analytic expressions for the moment integrals (2.8) in case of the measurement update, the measurement equation (5.5) is squared, which yields

d_i,^¡ρi

¢2

=¡S_i− t¢T·¡S_i− t¢ − 2·¡S_i− t¢T

· v_i+ v^T_i · v_i, (5.6)

166 5 Applications

where diis a squared range assumed to be calculated by ˆdi= ˆρ²_i. Thus, the modified measurement equation (5.6) can be described in short term via

d_i= hi¡t ,v_i¢

(5.7) for a single measurement to landmark i and via

d = h¡t ,v¢ (5.8)

for measurements to all landmarks, where di and hi(.,.) are the i th ele-ment of d and h(.,.), respectively. Hence, the vector of squared ranges ˆd is calculated according to ˆd = ˆr ¯ ˆr . The measurement noise v in (5.8) is land-mark. By assuming correlations between landmarks, an algorithm valid for many real-world application can be derived. The case of uncorrelated landmarks is a special case of the algorithm.

Due to the consideration of squared ranges, the measurement model in (5.6) is a polynomial of order two allowing closed-form calculations of the moment integrals (2.8). Hence, according to (2.9) the mean vector and covariance matrix of the posterior state estimate x_k∼N^¡_µ^e

k, C^e_k¢ are

5.1 Range-based Localization 167

respectively, whereµ^d_k (squared measurement mean), C^d_k (squared mea-surement covariance matrix), and C^xd_k (cross-covariance between state and squared measurement) are given by¹

µ^d_k = (V ¯ V)^T· 1_M+ 1_N· Tr¡P·C_k^p· P^T¢ + O^T· diag¡C^v¢ , C^d_k = O^T·¡4· ¡vec(V)·vec(V)^T¢ ¯ T + 2·T ¯ T¢ ·O , C^xd_k = −2 · C^p_k· P^T· V ,

(5.10)

respectively, with

S,^£S₁ ^{. . . S}_N^{¤ ,} P,£IM 0M¤ , V,S −¡1_N¢T

⊗¡P·µ_k^p¢ , O,^IN⊗ 1_M ,

T,^Cv+ 1N⊗¡P·C^p· P^T¢ ,

where vec(V) is the vectorized version of the matrix V, 1_N is a vector of ones of dimension N , and 1Nis a one matrix. The variable M = 3 stands for the three-dimensional space.

Example 24: Four Landmarks

The proposed analytical moment calculation (AMC) is compared against the EKF and UKF. For this purpose four landmarks with posi-tions

S =£S₁ . . . S₄¤ =





−2 −2 2 2

−2 2 −2 2

0 0 0 2



m

1 A detailed derivation can be found in Paper L.

168 5 Applications

(a) The average rmse and its stan-dard deviation.

(b) Mean of the determinant of the position covariance C^e_k,t.

Figure 5.2:Result of the three estimators AMC, UKF, and EKF for different noise levels.

are considered. The measurement noise covariance matrix of each landmark i is assumed to be C^v_i = I · σ²n withσn=⁽ⁿ⁻¹⁾/3m where n = 1...10, i.e., ten different noise levels are investigated. For each noise level 1000 MC runs are simulated, where each run consists of 100 measurement steps.

The initial state at time step k = 0 has zero mean and covariance C0= 10 · I6. The sampling time is T = 0.1s. The process noise covariance matrix comprises the elementsσ²c,x= σ²_c,y= 0.01, and σ²_c,z= 0.0001.

In Figure 5.2a, the average rmse is depicted. For small noise, all three filters perform similar. If the noise increases, the rmse of the EKF increases much stronger compared to the other two filters. For a high noise level, the UKF and the proposed approach present comparable results, where the average rmses and the standard deviations of the AMC are slightly smaller.

5.1 Range-based Localization 169

The average determinant of the covariance matrix of the position estimate t_k of all test runs is shown in Figure 5.2b. Due to the lin-earization based on first-order Taylor-series expansions, the deter-minant of the EKF is too small and thus the EKF is too certain about its estimate. Hence, the estimation results are inconsistent, which is often a problem when using an EKF. On the other hand, LRKFs or analytic approaches as the AMC overcome this problem. The determinant of the AMC is smaller compared to the determinant of the UKF. Furthermore, as described before, the rmse of the AMC is smaller as well. All together, the AMC is more informative compared to the UKF.

The computational complexity of the above closed-form measurement update is inO¡M³+ M²· N¢ for the meanµ^d_k, inO¡M²· N³¢ for the co-variance C^d_k, andO¡M²· N¢ for the cross-covariance C_k^xd, where typically M ¿ N . For calculating the required moments in (5.10), only vector-matrix products and no additional vector-matrix inversions or roots are required.

For comparison, already the computational complexity of calculating the matrix square root required for an LRKF is inO¡N³· M³¢.