Constraints with autocorrelated noise can be understood with

also seen that the augmented optimization problem is harder to solve as it becomes less sparse and more memory consuming. As a consequence, if we would implement these models within the core estimator, we would lose its block-tridiagonal structure and the associated advantages. Moreover, we have derived the AR(1) NLLSQ estimation proce-dure, but we do not have a graph-based representation for it. We are generally interested in such a representation to help us reason more intuitively about the underlying prob-lem structure. For this reason we have already derived in Section 5.3 a prior node to represent the effect of marginalization on our sliding window pose graph. In the same spirit we derive graph elements to represent constraints with autocorrelated noise in the following.

6.4.2. Constraints with autocorrelated noise can be

6.4. Measurements with autocorrelated errors 127

x_i

xj

x^w_k

x^w_l e^w,AR_m ,Ω^w,AR_m

e^w_k,Ω^w,AR_k e^w_l ,Ω^w,AR_l

Figure 6.10.: The same part of a pose graph as in Figure 6.8, but this time we assume the noise of the observed nodes to be modeled as AR(1). We need an additional constraint to model the influence of the autocorrelated noise.

shows the same part of the graph as Figure 6.8, but this time we assume the noise of the observed nodes to be described as AR(1). The most striking difference when comparing this to the graph in Figure 6.8 is the introduction of the hyperedgez^w,AR_m that connects both observed nodes and both hidden nodes simultaneously. Looking more closely, we also see that the edges from the observed to the hidden nodes have undergone a cor-rection of their associated information matrices. We derive in the following that these three edges have the same influence on the underlying NLLSQ formulation as directly augmenting the stochastic model in the optimization problem as we have done in Sec-tion 6.4.1. We conclude that these edges are therefore the graph-based representaSec-tion of constraints from measurements with autocorrelated noise.

The new hyperedge z^w,AR_m connects x^w_k,x^w_l ,x_i, and x_j. It depends on a new error functione^w,AR_m with

e^w,AR_m (x_i,x_j,z^w_k,z^w_l ) = e^w_k(x_i,z^w_k) +e^w_l (x_j,z^w_l ). (6.56) Its partial derivatives are

∂e^w,AR_m (xi,xj,z^w_k,z^w_l )

∂xi

= ∂e^w_k(xi,z^w_k)

∂xi

= ˘J_m,i^w,AR= ˘J_k,i^w, (6.57)

∂e^w,AR_m (xi,xj,z^w_k,z^w_l )

∂xj

= ∂e^w_l (xj,z^w_l )

∂xj

= ˘J_m,j^w,AR = ˘J_l,j^w. (6.58)

Its Jacobian is therefore

J˘_m^w,AR= h ^{i j}

· · · 0 ^{∂ew,ARm (x}_∂x^{i,xj,zwk,zwl)}

i

x=˘x 0 · · · 0 ^{∂ew,ARm (x}_∂x^{i,xj,zwk,zwl)}

j

x=˘x 0 · · · i (6.59)

= h ^{i j}

· · · 0 J˘_k,i^w 0 · · · 0 J˘_l,j^w 0 · · · i

. (6.60)

The corresponding constraintz^w,AR_m takes into account this error function and the infor-mation matrixΛ^w,AR_m =Λ^w,AR_kl such that the squared error becomes

e^w,AR_m =e^w,AR_m (xi,xj,z^w_k,z^w_l )^>Λ^w,AR_m e^w,AR_m (xi,xj,z^w_k,z^w_l ). (6.61) This constraint leads toH_m^w,ARwith

H_m^w,AR =









 ...

( ˘J_k,i^w)^>Λ^w,AR_m J˘_k,i^w · · · ( ˘J_k,i^w)^>Λ^w,AR_m J˘_l,j^w

... ... ...

( ˘J_l,j^w)^>Λ^w,AR_m J˘_k,i^w · · · ( ˘J_l,j^w)^>Λ^w,AR_m J˘_l,j^w ...











, (6.62)

andb^w,AR_m with

b^w,AR_m =











...

( ˘J_k,i^w)^>Λ^w,AR_m ˘e^w,AR_m ...

( ˘J_l,j^w)^>Λ^w,AR_m e˘^w,AR_m ...











. (6.63)

This is a nice intermediate result as it leads to the correct addends inH_ij^w,ARandH_ji^w,AR, but because of the information matrix Λ^w,AR_m it leads to slightly wrong addends in H_ii^w,AR, H_jj^w,AR, b^w,AR_i , and b^w,AR_j . We correct for this by considering the other two mentioned edges z^w_k and z^w_l . In terms of the error functions, we use them as ordinary

6.4. Measurements with autocorrelated errors 129 edges between observed and hidden nodes, just as we would model them when assum-ing AWGN noise. However, we adapt the associated information matrices to be

Λ^w_k =Λ^w,AR_k −Λ^w,AR_m , (6.64) Λ^w_l =Λ^w,AR_l −Λ^w,AR_m . (6.65) The two constraints z^w_k and z^w_l lead with these corrected information matrices to H_k^w andH_l^wwith

H_k^w =





 ...

( ˘J_k,i^w)^>Λ^w_kJ˘_k,i^w ...





, (6.66)

H_l^w =





 ...

( ˘J_l,j^w)^>Λ^w_l J˘_l,j^w ...





. (6.67)

Equivalently we find that

b^w_k =





 ...

( ˘J_k,i^w)^>Λ^w_ke˘^w_k ...





, (6.68)

b^w_l =





 ...

( ˘J_k,i^w)^>Λ^w_l e˘^w_l ...





. (6.69)

These three constraints together allow us to compute H^w,AR and b^w,AR for the graph depicted in Figure 6.10:

H^w,AR =H_k^w+H_l^w+H_m^w,AR, (6.70) b^w,AR =b^w_k +b^w_l +b^w,AR_m . (6.71) (6.72)

xi

xj

xk

e^v,AR_n ,Ω^v,AR_n e^vl,Ω^v

,AR l

e_m^v,Ω^v,AR m

Figure 6.11.: The same part of a pose graph as in Figure 6.9, but this time we assume the noise of the odometry edges to be modeled as AR(1). We need an addi-tional constraint to model the influence of the noise of the AR(1) process.

It is straightforward to verify that these results are equal to (6.34) and (6.35). Therefore, we have defined a graph-based representation for modeling observed nodes with auto-correlated noise that is equal to the direct modeling as a AR(1) NLLSQ problem. A key piece that we are still missing is, how a graph-based representation of odometry edges with autocorrelated noise looks like. We approach this question in the following.

Odometry edges Within a similar train of thought as the derivation of a graph-based representation for observed nodes with autocorrelated noise, it is now our goal to derive the same for odometry edges. To this end, we begin by stating that the graph-based representation in Figure 6.11 solves this problem. With a similar approach as for observed nodes, we first define the new hyperedgez^v,AR_n and then show that we need to adapt the information matrices of the usual constraintsz^v_l andz^v_mto achieve the desired result.

The constraintz^v,AR_n of the corresponding hyperedge is associated to the error func-tion

e^v,AR_n (xi,xj,xk,z^v_l,z^v_m) =e^v_l(xi,xj,z^v_l) +e^v_m(xj,xk,z^v_m). (6.73) Its partial derivatives with respect to the state variables are

∂e^v,AR_n (xi,xj,xk,z^v_l,z^v_m)

∂xi

= ∂e^v_l(xi,xj,z^w_l )

∂xi

, (6.74)

∂e^v,AR_n (xi,xj,xk,z^v_l,z^v_m)

∂x_j = ∂e^v_l(xi,xj,z^w_l )

∂x_j +∂e^v_m(xj,xk,z^w_m)

∂x_j , (6.75)

∂e^v,AR_n (xi,xj,xk,z^v_l,z^v_m)

∂x_k = ∂e^v_m(xj,xk,z^w_m)

∂x_k . (6.76)

With the usual abbreviations the partial derivatives evaluated at the linearization point

6.4. Measurements with autocorrelated errors 131 are

J˘_n,i^v,AR= ˘J_l,i^v, (6.77)

J˘_n,j^v,AR= ˘J_l,j^v + ˘J_m,j^v , (6.78)

J˘_n,k^v,AR= ˘J_m,k^v . (6.79)

The Jacobian of the constraintz^v,AR_n is therefore

J˘_n^v,AR = h ^{i j k}

· · · 0 J˘_n,i^v,AR 0 · · · 0 J˘_n,j^v,AR 0 · · · 0 J˘_n,k^v,AR 0 · · · i . (6.80) The constraintz^v,AR_n leads toH_n^v,ARwith

H_n^v,AR= ( ˘J_n^v,AR)^>Λ^v,ARJ˘_n^v,AR = (6.81)















 ...

( ˘J_n,i^v,AR)^>Λ^v,AR_n J˘_n,i^v,AR · · · ( ˘J_n,i^v,AR)^>Λ^v,AR_n J˘_n,j^v,AR · · · ( ˘J_n,i^v,AR)^>Λ^v,AR_n J˘_n,k^v,AR

... ... ...

( ˘J_n,j^v,AR)^>Λ^v,AR_n J˘_n,i^v,AR · · · ( ˘J_n,j^v,AR)^>Λ^v,AR_n J˘_n,j^v,AR · · · ( ˘J_n,j^v,AR)^>Λ^v,AR_n J˘_n,k^v,AR

... ... ...

( ˘J_n,k^v,AR)^>Λ^v,AR_n J˘_n,i^v,AR · · · ( ˘J_n,k^v,AR)^>Λ^v,AR_n J˘_n,j^v,AR · · · ( ˘J_n,k^v,AR)^>Λ^v,AR_n J˘_n,k^v,AR ...

















(6.82) andb^v,AR_n with

b^v,AR_n =

















...

( ˘J_n,i^v,AR)^>Λ^v,AR_n e˘^v,AR_n ...

( ˘J_n,j^v,AR)^>Λ^v,AR_n e˘^v,AR_n ...

( ˘J_n,k^v,AR)^>Λ^v,AR_n e˘^v,AR_n ...

















. (6.83)

Using the definition of the Jacobians we develop (6.82) toH_n^v,ARwith the entries H_n,ii^v,AR= ( ˘J_l,i^v)^>Λ^v,AR_n J˘_l,i^v, (6.84) H_n,ij^v,AR= ( ˘J_l,i^v)^>Λ^v,AR_n J˘_l,j^v + ( ˘J_l,i^v)^>Λ^v,AR_n J˘_m,j^v , (6.85) H_n,ik^v,AR= ( ˘J_l,i^v)^>Λ^v,AR_n J˘_m,k^v , (6.86)

H_n,ji^v,AR= (H_n,ij^v,AR)^>, (6.87)

H_n,jj^v,AR= ( ˘J_l,j^v)^>(Λ^v,AR_n J˘_l,j^v +Λ^v,AR_n J˘_m,j^v ) + ( ˘J_m,j^v )^>(Λ^v,AR_n J˘_m,j^v +Λ^v,AR_n J˘_l,j^v ), (6.88) H_n,jk^v,AR= ( ˘J_m,j^v )^>Λ^v,AR_n J˘_m,k^v + ( ˘J_l,j^v)^>Λ^v,AR_n J˘_m,k^v , (6.89)

H_n,ki^v,AR= (H_n,ik^v,AR)^>, (6.90)

H_n,kj^v,AR= (H_n,jk^v,AR)^>, (6.91)

H_n,kk^v,AR= ( ˘J_m,k^v )^>Λ^v,AR_n J˘_m,k^v (6.92) and similarly, (6.83) to

b^v,AR_n =

















...

( ˘J_l,i^v)^>Λ^v,AR_n ˘e^v_l + ( ˘J_l,i^v)^>Λ^v,AR_n e˘^v_m ...

( ˘J_l,j^v)^>Λ^v,AR_n ˘e^v_l + ( ˘J_l,j^v)^>Λ^v,AR_n e˘^v_m+ ( ˘J_m,j^v )^>Λ^v,AR_n e˘^v_m+ ( ˘J_m,j^v )^>Λ^v,AR_n ˘e^v_l ...

( ˘J_m,k^v )^>Λ^v,AR_n ˘e^v_m+ ( ˘J_m,k^v )^>Λ^v,AR_n ˘e^v_l ...















 .

(6.93)

As in the derivation for the graph-based representation of observed nodes with auto-correlated noise, we notice that some of these matrix entries are already correct, while others need some modification. To be more precise, the entries underlined in blue are not yet identical when comparing them to (6.45) and (6.55), which is our goal. This is caused by the deviation of the information matrices. We correct for this by adapting the

6.4. Measurements with autocorrelated errors 133 information matrices of the constraintsz^v_l andz^v_mto

Λ^v_l =Λ^v,AR_l −Λ^v,AR_n , (6.94) Λ^v_m =Λ^v,AR_m −Λ^v,AR_n . (6.95) These two constraints then lead toH_l^vandH_m^v with

H_l^v=









 ...

( ˘J_l,i^v)^>Λ^v_lJ˘_l,i^v · · · ( ˘J_l,i^v)^>Λ^v_lJ˘_l,j^v

... ...

( ˘J_l,j^v)^>Λ^v_lJ˘_l,i^v · · · ( ˘J_l,j^v )^>Λ^v_lJ˘_l,j^v ...











(6.96)

and

H_m^v =









 ...

( ˘J_m,j^v )^>Λ^v_mJ˘_m,j^v · · · ( ˘J_m,j^v )^>Λ^v_mJ˘_m,k^v

... ...

( ˘J_m,k^v )^>Λ^v_mJ˘_m,j^v · · · ( ˘J_m,k^v )^>Λ^v_mJ˘_m,k^v ...











. (6.97)

The corresponding right-hand side vectors areb^v_l andb^v_m with

b^v_l =











...

( ˘J_l,i^v)^>Λ^v,AR_l ˘e^v_l ...

( ˘J_l,j^v)^>Λ^v,AR_l ˘e^v_l ...











, (6.98)

b^v_m =











...

( ˘J_m,j^v )^>Λ^v,AR_m ˘e^v_m ...

( ˘J_m,k^v )^>Λ^v,AR_m e˘^v_m ...











. (6.99)

In total, the three constraintsz^v_l,z^v_m, andz^v,AR_n allow us to computeH^v,ARandb^v,ARfor the graph depicted in Figure 6.11 to

H^v,AR=H_l^v+H_m^v +H_n^v,AR, (6.100) b^v,AR=b^v_l +b^v_m+b^v,AR_n . (6.101) We compare the result with (6.45) and (6.55) and verify that they are indeed identi-cal. We conclude that the given three constraints are a graph-based representation for odometry edges with autocorrelated noise. Together with the graph-based representa-tion for observed nodes with autocorrelated noise, we are now able to understand the influence of this kind of noise on the graph-based representation. This by itself is a big step forward. However, as these new constraints lead to the exact same entries as AR(1) NLLSQ, we still suffer from the increased computational demand. In the next section we therefore tackle this challenge.

6.4. Measurements with autocorrelated errors 135

6.4.3. AR(1) scaling: efficient implementation by scaling

Im Dokument Sensor fusion for localization of automated vehicles (Seite 141-150)