Sensor Fusion, 2014 Lecture 1: 1 Lectures

Transcription

1 Sensor Fusion, 2014 Lecture 1: 1 Lectures Lecture Content 1 Course overview. Estimation theory for linear models. 2 Estimation theory for nonlinear models 3 Sensor networks and detection theory 4 Nonlinear filter theory. The Kalman filter. Filter banks. 5 Kalman filter approximation for nonlinear models (EKF, UKF). 6 The point-mass filter and the particle filter. 7 The particle filter theory. The marginalized particle filter. 8 Simultaneous localization and mapping (SLAM). 9 Modeling and motion models. 10 Sensors and filter validation.

2 Sensor Fusion, 2014 Lecture 1: 2 Course Material Literature: Statistical Sensor Fusion. Studentlitteratur, Exercises: compendium Laboration 1 and 2 compendium (links on homepage) Matlab toolbox (link on homepage) Signal and Systems Lab. Toolbox manual: compendium Android app SensorFusionApp and Matlab and Java real-time interface files. Optional ios app: xsensor (from Appstore), with matlab interface xbow2sig in the toolbox.

3 Sensor Fusion, 2014 Lecture 1: 3 Changes from 2013 The following changes have been made: Lab 2 updated based on feedback from first year Minor updates in exercise compendium Manual to the toolbox now as a compendium

4 Sensor Fusion, 2014 Lecture 1: 4 Lecture 1: Linear estimation theory and sensor networks Whiteboard: Basic definitions and derivations. Slides: Summaries Examples Code examples Algorithms Goal: localization of a device, a user or an object in a sensor network!

5 Sensor Fusion, 2014 Lecture 1: 5 A simple example Triangulation, as used by seamen for a long time. Assume two sensors that each measure bearing to target accurately, and range less accurately (or vice versa). How to fuse these measurements? x S1 S x1 Use all four measurements with the same weight LS. Poor range information may destroy the estimate. Discard uncertain range information, gives a triangulation approach with algebraic solution. Weigh the information according to its precision WLS.

6 Sensor Fusion, 2014 Lecture 1: 6 A standard example Laboration 1, but similar principles used in radio and underwater applications. Vehicle sends out regular acoustic pings. Time synchronized microphones detect ping arrival times. Localization: Can the vehicle be located if the microphone positions are known? If the ping times are known? If the ping times are unknown? Mapping: Can the microphone positions be estimated if the vehicle position is unknown? Simultaneous Localization and Mapping: can both the target and microphone positions be estimated?

7 Sensor Fusion, 2014 Lecture 1: 7 Another standard example WiFi base stations in indoor environment. Receiver gets BSSI (identity) and RSS (received signal strength). Can the receiver be positioned if the WiFi base station positions are known? Can the receiver be positioned if the WiFi base station positions are unknown, but there is a map over RSS as a function of position (fingerprinting)?

8 Sensor Fusion, 2014 Lecture 1: 8 A hard example A speaker sends out a 10kHz (120dB!) tone. Several vehicles pass by. A microphone network computes Doppler frequencies. Can the targets speeds be estimated. Can the targets positions be estimated.

9 Sensor Fusion, 2014 Lecture 1: 9 Chapter 2 overview Linear model y k = H k x + e k WLS theory WLS off-line versus on-line forms The fusion formula (and safe fusion) Overview of algorithms with examples Tricks: transform to linear model and conditionally linear model

10 Sensor Fusion, 2014 Lecture 1: 10 WLS Summary Linear model: WLS loss function V WLS (x) = k=1 y k = H k x + e k, Cov(e k ) = R k, k = 1,..., N, y = Hx + e, Cov(e) = R. N (y k H k x) T R 1 (y k H k x) = (y Hx) T R 1 (y Hx). k=1 k=1 k Solution in batch form ( N ) 1 ˆx = Hk T R 1 k H N k Hk T R 1 k y k = ( H T R 1 H ) 1 H T R 1 y, P = ( N k=1 H T k R 1 k H k) 1 = ( H T R 1 H ) 1.

11 Sensor Fusion, 2014 Lecture 1: 11 Sequential WLS The WLS estimate can be computed recursively in the space/time sequence y k. Suppose the estimate ˆx k 1 with covariance P k are based on observations y 1:k 1, where we initiate with ˆx 0 and P 0 (a prior ). A new observation is fused using ( 1 ˆx k = ˆx k 1 + P k 1 Hk T H k P k 1 Hk k) T + R (yk H k ˆx k 1 ), ( 1 P k = P k 1 P k 1 Hk T H k P k 1 Hk k) T + R Hk P k 1. Note that the fusion formula can be used alternatively. In fact, the derivation is based on the information fusion formula applying the matrix inversion lemma.

12 Sensor Fusion, 2014 Lecture 1: 12 The fusion formula The fusion formula for two independent estimates is E(ˆx 1 ) = E(ˆx 2 ) = x, Cov(ˆx 1 ) = P 1, Cov(ˆx 2 ) = P 2 ˆx = P ( P1 1 ˆx 1 + P2 1 ˆx 2), P = ( P1 1 + P2 1 ) 1. If the estimates are not independent, the safe fusion (or covariance intersection algorithm) provides a pessimistic lower bound accounting for worst case correlation.

13 Sensor Fusion, 2014 Lecture 1: 13 Simple sensor network example, continued Code for triangulation in Signal and Systems Lab: p1 =[0;0]; p2 =[2;0]; x =[1;1]; X1= ndist (x,0.1*[1-0.8; ]) ; X2= ndist (x,0.1*[1 0.8;0.8 1]) ; plot2 (X1,X2) x S1 S x1

14 Sensor Fusion, 2014 Lecture 1: 14 Sensor network example, continued X3= fusion (X1,X2); % WLS X4 =0.5* X1 +0.5* X2; % LS plot2 (X4,X3) x S1 S x1

15 Sensor Fusion, 2014 Lecture 1: 15 Safe fusion Given two unbiased estimates ˆx 1, ˆx 2 with information I 1 = P1 1 and I 2 = P2 1 (pseudo-inverses if singular covariances), respectively. Compute the following: 1. SVD: I 1 = U 1 D 1 U1 T. 2. SVD: D 1/2 1 U1 T I 2U 1 D 1/2 1 = U 2 D 2 U2 T. 3. Transformation matrix: T = U2 T D1/2 1 U State transformation: ˆ x 1 = T ˆx 1 and ˆ x 2 = T ˆx 2. The covariances of these are Cov(ˆ x 1 ) = I and Cov(ˆ x 2 ) = D2 1, respectively. 5. For each component i = 1, 2,..., n x, let 6. Inverse state transformation: ˆ x i = ˆ x 1, i D ii = 1 if D2 ii < 1, ˆ x i = ˆ x 2, i D ii = D2 ii if D2 ii > 1. ˆx = T 1ˆ x, P = T 1 D 1 T T

16 Sensor Fusion, 2014 Lecture 1: 16 Transformation steps ˆx 2 ˆx 1 ˆx 2 ˆx 1 ˆx 2 ˆx 1

17 Sensor Fusion, 2014 Lecture 1: 17 Sensor network example, cont d X3= fusion (X1,X2); % WLS X4= fusion (X1,X3); % X1 used twice X5= safefusion (X1,X3); plot2 (X3,X4,X5)

18 Sensor Fusion, 2014 Lecture 1: 18 Summary Lecture 1 Linear model on batch form: WLS minimizes the loss function WLS solution y = Hx + e, Cov(e) = R. V WLS (x) = (y Hx) T R 1 (y Hx). ˆx = ( H T R 1 H ) 1 H T R 1 y, P = ( H T R 1 H ) 1. LS special case with R = I and gives larger P. The fusion formula for two independent estimates is E(ˆx 1 ) = E(ˆx 2 ) = x, Cov(ˆx 1 ) = P 1, Cov(ˆx 2 ) = P 2 ˆx = P ( P1 1 ˆx 1 + P2 1 ˆx ) ( 2, P = P P2 1 ) 1. If the estimates are not independent, P is larger than indicated.