n j u = (3) b u Then we select m j u as a cross product between n j u and û j to create an orthonormal basis: m j u = n j u û j (4)

Size: px

Start display at page:

Download "n j u = (3) b u Then we select m j u as a cross product between n j u and û j to create an orthonormal basis: m j u = n j u û j (4)"

Karin Douglas
5 years ago
Views:

1 4 A Position error covariance for sface feate points For each sface feate point j, we first compute the normal û j by usin 9 of the neihborin points to fit a plane In order to create a 3D error ellipsoid for the sface feate point, we create an orthonormal basis with the correspondin normal We select a vector n j u that is perpendicular to û j as follows: n j u = û j 3 û j 2 (3) Then we select m j u as a cross product between n j u and û j to create an orthonormal basis: m j u = n j u û j (4) Fi 5: LiDAR-3D city model matchin in two different ban areas We bein with a rid of initial position uesses (red) around the true position (black) In (a) and (b), there are adequate feates The position estimates after matchin (blue) convere to the true position In (c) and (d), the feate distribution is relatively poor The position estimates after matchin (blue) are parallel to the buildin sface sroundin feates In ban environments, we typically observe structed objects such as buildins Hence, we focus primarily on sface and ede feates in the point cloud We extract these feate points based on the cvate at each point, as described in 2 The alorithm can be summarized in the followin steps: For each point in the point cloud, compute the cvate c i as follows 2: c i = 1 X i (X i X j ), (2) jɛs,j i X i : Coordinates of i th point relative to the LiDAR S : Small neihborhood of points near i Discard stray points and points on occluded objects These are detected based on the rane and distance between consecutive points Divide the point cloud into 4 equal sections For each section, sort the cvate values Points with the lowest cvate values are assumed to be on a plane and are classified as sface points Points with the hihest cvate values are assumed to be at an ede or a corner and are classified toether as ede points If a point is classified as either sface or an ede point, its neihborin points are discarded This is done in order to prevent multiple detections of the same feates (d) We normalize n j u and m j u to obtain the followin basis: {û j, n j u, m j u} n j u = nj u n j u m j u = mj u m j u After creatin the orthonormal basis, we proceed to create the error covariance matrix We use the basis as o eienvectors: Vu j = û j n j u m j u We model the error covariance ellipsoid with the hypothesis that each sface feate point contributes in reducin position error in the direction of the correspondin sface normal Additionally, we assume that sface points closer to the LiDAR are more reliable than those fther away, because of the density of points Hence, we use the followin eienvalues correspondin to the eienvectors in (6): L j u = d j u (5) (6) au b u, (7) b u a u, b u : Constants for all sface points such that: a u b u d j u : Distance of the j th sface point from the LiDAR The values for constants a u and b u are tuned din implementation Finally, usin the eienvectors V j u and the eienvalues L j u, we construct the position error covariance matrix for the sface point as follows: R j u = Vu j L j u Vu j 1 Fie 6(a) shows a sample error ellipsoid for a sface point enerated by (8) (8)

5 L j e, we construct the position error covariance matrix for the ede point as follows: R j e = Ve j L j e Ve j 1 (13) Fie 6(b) shows a sample error ellipsoid for an ede point enerated by (13) (a)

n j u and m j u complete orthonormal basis (b) shows the covariance ellipsoid R j e contributed by the j th ede point ê j represents the correspondin ede vector; and n j e and m j e complete

below j If the distance between j and the closest ede points is below a threshold, we use the points to estimate the ede orientation Thus we obtain the ede vector ê j Next, we follow similar steps as

2 5 L j e, we construct the position error covariance matrix for the ede point as follows: R j e = Ve j L j e Ve j 1 (13) Fie 6(b) shows a sample error ellipsoid for an ede point enerated by (13) (a) Fi 6: Position error covariance ellipsoid for sface and ede feate points (a) shows the covariance ellipsoid R j u contributed by the j th sface point û j represents the correspondin sface normal; and n j u and m j u complete orthonormal basis (b) shows the covariance ellipsoid R j e contributed by the j th ede point ê j represents the correspondin ede vector; and n j e and m j e complete orthonormal basis B Position error covariance for ede feate points For each ede feate point j, we first find the orientation of the ede We find the closest ede points in both the scans above and below j If the distance between j and the closest ede points is below a threshold, we use the points to estimate the ede orientation Thus we obtain the ede vector ê j Next, we follow similar steps as we did for each sface point We first create an orthonormal basis with ê j : n j e = ê j 3 ê j 2 (9) (b) m j e = n j e ê j (1) After normalizin n j e and m j e as done in (5), we obtain the required basis {ê j, n j e, m j e}, which we use as the eienvectors: Ve j = ê j n j e m j e (11) We model the error covariance ellipsoid with the hypothesis that each ede feate point helps in reducin position error in the directions perpendicular to the ede vector A vertical ede, for example, would help in reducin horizontal position error Additionally, we assume that ede points closer to the LiDAR are more reliable than those fther away, because of the density of points Hence, we use the followin eienvalues correspondin to the eienvectors in (11): ae L j e = d j e b e, (12) b e a e, b e : Constants for all ede points such that: a e b e d j e : Distance of the j th ede point from the LiDAR The values for constants a e and b e are tuned din implementation Finally, usin the eienvectors V j e and the eienvalues C Combinin error ellipsoids In order to obtain the overall position error covariance, we combine the error covariance matrices for all the individual sface and ede feate points The combined covariance matrix can be obtained as 45: R L = n u R j 1 u j=1 + n e R j 1 e j=1 1, (14) where R j e and R j u are the covariance matrices from individual sface (8) and ede (13) feate points respectively; n u and n e are the number of sface and ede feate points in the point cloud Fie 7 shows the resultin position error covariance ellipsoids R L for two point clouds in ban environments In the horizontal plane, the covariance ellipsoid is larer in the direction parallel to the buildin sides In the vertical direction, the size of the covariance ellipsoid remains constrained due to points detected on the round V GPS MEASUREMENT MODEL To create the GPS measement model, we use the pseudorane measements The measement between a GPS receiver u and the k th satellite can be modelled as 46: ρ k u = r k u + cδt u δt k + I k ρ u + T k ρ u + ɛ k ρ u, (15) Fi 7: Overall position error ellipsoids R L, for two point clouds enerated by the on-board LiDAR in an ban environment

6 r k u : True rane between receiver u and k th satellite c : Speed of liht δt u : Clock bias for receiver u δt k : Clock bias for k th satellite I k ρ u T k ρ u ɛ k ρ u : Ionospheric error :

3 6 r k u : True rane between receiver u and k th satellite c : Speed of liht δt u : Clock bias for receiver u δt k : Clock bias for k th satellite I k ρ u T k ρ u ɛ k ρ u : Ionospheric error : Tropospheric error : Noise in pseudorane measement In order to eliminate certain error terms, we use doubledifference pseudorane measements, which are calculated by differencin the pseudorane measements between two satellites and between two receivers The double difference pseudorane measements between two satellites k and l, and between two GPS receivers u and r can be represented as: ρ k l = (ρ k u ρ k r) (ρ l u ρ l r) (r k + cδt + ɛ k ρ ) (r l + cδt + ɛ l ρ ) = (r k r l ) + ɛ k l ρ, (16) where for short baselines between the two receivers, the ionospheric and tropospheric errors can be assumed to be neliible Fthermore, the baseline between the two receivers can be assumed to be sinificantly smaller than the distance between the receivers and the satellites Thus, the rane term r k in (16) can be approximated as: r k = r k u r k r 1 k r x, (17) where, x is the baseline between the two receivers; and 1 k r is a unit vector from the receiver r to the satellite k as shown in Fie 8 Usin (17), the above expression for the double difference pseudorane measement can be expressed as follows: A NLOS satellites elimination Before proceedin to create the vector of GPS double difference pseudorane measements, we check if any of the satellites detected by the receiver are non-line-of-siht sinals We use the 3D city model described in section III-A to detect the NLOS satellites We use the position output enerated by the LiDAR-3D city model matchin, as described in section III, to locate the receiver on the 3D city model Next, we draw LOS vectors from the receiver to every satellite detected by the receiver and eliminate satellites whose correspondin LOS vectors intersect the 3D city model Fi 9 shows the above implementation in an ban scenario B GPS measement vector and covariance After eliminatin the NLOS satellites, we select satellites that are visible to both the user and the reference receivers to create the GPS measement vector and its covariance The vector of double-difference pseudorane measements between the receivers can be represented as: ρ DD = ρ 1 K ρ 2 K ρ (K 1) K ρ 1 u ρ 2 ụ = A ρ DD K u ρ 1 r ρ 2 r ρ K r, (19) where A DD relates the individual pseudorane measements to the double-difference measements based on (16) ρ k l (1 k r 1 l r) x + ɛ k l ρ (18) Fi 8: Geometry of sinle-difference measements Here x u and x r are the positions of the user and reference receivers in the Earth Centered Earth Fixed (ECEF) frame 46 The baseline between these receivers x, is assumed to be sinificantly smaller than the ranes to the satellites r k u and r k r Fi 9: Elimination of NLOS satellite sinals The receiver position (black) is projected on the 3D city model LOS vectors are drawn to all detected satellites: SV3, SV14, SV16, SV22, SV23, SV26, SV31 The LOS vectors to satellites SV23 and SV31 intersect (red) the 3D city model and are eliminated from fther calculations

7 Once we have the vector of GPS measements, we proceed to model its covariance We assume that the individual pseudorane measements are independent, and that the variance for each measement is a

4 7 Once we have the vector of GPS measements, we proceed to model its covariance We assume that the individual pseudorane measements are independent, and that the variance for each measement is a function of the correspondin SNR (C/N ) k u in db, where k refers to the k th satellites and u refers to the user receiver 47 We use the followin covariance matrices for the user and the reference receivers respectively: R ρu = dia(1 (C/N ) 1 u 1, 1 (C/N )2 u 1,, 1 (C/N )K u 1 ) (2) R ρr = dia(1 (C/N ) 1 r 1, 1 (C/N )2 r 1,, 1 (C/N )K r 1 ) (21) To obtain the covariance matrix for the double-difference measements, we propaate the above covariance matrices as follows: R ρ DD = A DD Rρu R ρr A DD T (22) VI GPS-LIDAR INTEGRATION A Unscented kalman filter structe In addition to usin a LiDAR and a GPS receiver, we use an IMU on-board the UAV Fie 1 shows the different frames of reference used in the filter The state vector consists of the followin states: x T = p u T, v ut, q u T, b T ω, b T a, q i T, (23) where p u, v u, and q u are the position, velocity and the orientation respectively of the UAV in the local GPS frame; b ω and b a are the IMU yroscope and accelerometer biases; q i is the orientation offset between the local GPS and IMU frames For the prediction step of the filter, we use a constant velocity model Additionally, we include the anular velocity ω m, and acceleration a m measements from the IMU in the prediction step 48, instead of the relatively expensive correction step Thus, the prediction step can be written as: p u v u v u q R T u (q b = u )ba R 5Ξ (q u )b ω + 5Ξ (q u ) T (q u ω m + ) a m, (24) b ω a q i }{{} }{{}}{{}}{{}}{{}}{{} f 1(x) u 1 f f 2(x) u 2 (x) ẋ where R (q u ) represents the rotation between the UAV frame and the local GPS frame; Ξ (q u ) expresses the time rate of chane of (q u ) 49 (24) can be re-written compactly as: ẋ = f (x) + f 1 (x)u 1 + f 2 (x)u 2, (25) with ẋ, f, f 1, f 2, u 1 and u 2 as marked in (24) For the correction step, we use pose information from the LiDAR, orientation information from the IMU and position information from the GPS receiver From the LiDAR we use pose information from two soces: one from the LiDAR odometry described in section II, and the other from the LiDAR - 3D city model matchin described in section III The Fi 1: Frames of reference The local GPS frame (blue) refers to the local North-East-Down (NED) frame centered at position where UAV beins operation The IMU frame (red) is slihtly offset the local GPS frame due to biases in the manetometers The UAV frame (reen) is fixed to the body of the UAV LiDAR odometry measements relate to the state variables as follows: h 1 = R (q u )(p u p u ) (26) h 2 = R (q u )R T (q u ), (27) where p u and q u refer to the previous filter estimate of position and orientation of the UAV in the local GPS frame The LiDAR - 3D city model matchin measements directly relate to the state variables: h 3 = p u (28) h 4 = q u (29) The orientation measement from the IMU is with respect to frame that is offset with respect to the local GPS frame Thus, it relates to the state variables as follows: h 5 = q i q u (3) For measements from the GPS receiver, we use the doubledifference measements ρ DD from (19) These measements can be related to the state variables as follows: h 6 = G 1 ((p u ) ECEF p ref ) h 7 = G 2 ((p u ) ECEF p ref ) h (K 1)+5 = G K 1 ((p u ) ECEF p ref ), (31) where K is the number of satellites; (p u ) ECEF and p ref are the positions of the UAV and the reference receiver in the ECEF frame 46; G is a function of the unit vectors from the reference receiver to the satellites bein used in the doubledifference calculation, as shown in (18) The measements h 1 and h 2 are in the LiDAR frame Thus, for the covariance of h 1, we use R L from (14) The measements h 3 and h 4 are from LiDAR - 3D city model matchin,

5 8 which are in the local GPS frame Thus, for covariance of h 3, we rotate R L to the local GPS frame as follows 5: R GP S L = R T (q u ) R L R (q u ) (32) For the orientation measements h 2, h 4 and h 5, we use a fixed diaonal matrix For the GPS double-difference measements, we use the covariance matrix R ρ DD from (22) B Non-linear observability analysis of filter To ense that all the state variables are observable, we check the observability of o system usin Lie derivatives 48, 49, 51 The zeroth order Lie derivative of a function h, is calculated as: L h(x) = h(x) (33) Fther derivatives of h, with respect to a function f, is calculated recsively as: L k f h(x) = (Lk 1 f h(x)) f(x) (34) x We use (33) and (34) on the measements (26)-(31) to obtain the necessary Lie derivatives for the observation matrix: O= L h 1 L h 2 L h 3 L h 4 L h 5 L h 6 L h 7 L h 8 L 1 f h 1 L 1 f h 2 L 2 f f h 1 = R (q u ) a 2,3 3 4 I 3 3 I 4 4 a 5,3 4 4 a 5,6 4 4 a 6,1 1 3 a 7,1 1 3 a 8,1 1 3 R (q u ) a 1,3 3 4 a 1,4 3 3 a 11,3 3 4 a 11,5 3 3 (35) In the above matrix, we can see that L h 1, L h 3 or L h 6, L h 7, L h 8 toether, account for the observability of the position states p u The observability of the velocity v u, is accounted for by L 1 f h 1 Additionally, L 1 f h 3 or L 1 f h 6, L 1 f h 7, L 1 f h 8 toether, would also make v u observable, but have not been included in (35) to keep the matrix concise L h 2 or L h 4, account for the observability of the orientation q u The biases b ω and b a are observable due to L 1 f h 2 and L 2 f f h 1 Finally, the orientation offset q i is observable due to L h 5 For practical applications, there miht be cases where the 3D city model is not available, thus makin measements h 3 and h 4 unavailable Fthermore, while naviatin throuh dense ban environments, the GPS measements h 6, h 7, h 8 miht be unavailable In these cases, theoretically the filter states would still be observable with the measements h 1, h 2 and h 5 However, h 3, h 4, h 6, h 7 and h 8 act as corrections to prevent the state variables from driftin

GPS-LiDAR Sensor Fusion Aided by 3D City Models for UAVs Akshay Shetty and Grace Xingxin Gao

GPS-LiDAR Sensor Fusion Aided by 3D City Models for UAVs Akshay Shetty and Grace Xingxin Gao SCPNT, November 2017 Positioning in Urban Areas GPS signals blocked or reflected Additional sensors: LiDAR,