Solutions for examination in Sensor Fusion,

Transcription

1 Solutions for examination in Sensor Fusion, (a By conditioning on x k and y k, and the matching noise components w 1,k and w,k, the remaining model becomes ( x l T k+1 = xl k + w l T k, where x l k = (vx k, vy k T and w l k = (w,k, w,k T. Note, that the model lack observations. With the observed linear Gaussian structure, the Rao-Blackwellized PF is applicable. (b Use a likelihood proposal! A remedy for depletion in a PF is to use a better proposal distribution. The optimal proposal turns into the likelihood function as the SNR increases, given that the observation model can be inverted. The conditions given in the questions fits this well. (c Since nothing is known about the correlation between x 1 and x, safe fusion must to be used to avoid double counting of information. Alternative 1: The covariance matrices are diagonal, hence safe fusion (Alg..1 turns into picking the estimate with the smallest covariance: (( ( 1 x c N,. 1 1 Alternative : A numerical solution can also be obtained using the SigSys toolbox: xhat1 = [1 1] ; P1 = eye ( ; xhat = [ ] ; P = [1/ ; ]; X1 = ndist ( xhat1, P1; X = ndist ( xhat, P; Xhat_c = safefusion ( X1, X (d The additional information given implies that x 1 and x are indepent. The optimal estimate is hence obtained using the sensor fusion formula. Alternative 1: Using the sensor fusion formula (Eq. (. x d N (ˆx d, P d, ˆx d = P d (P 1ˆx 1 + P ˆx = (, P d = (P P 1 1 = ( 1. Alternative : A numerical solution can also be obtained using the SigSys toolbox: Xhat_d = fusion ( X1, X (e Make Q larger! The speed of the filter deps on the ratio Q / R ; the larger the the ratio, the faster the filter adapts the estimate to the measurements. As mentioned in the question, the measurement noise is obtained through a calibration phase, and can therefore be assumed to be fairly accurate. Hence, to increase the ratio, Q should be made larger, as R is assumed correct. 1

2 . load ( data17 y = sig ( ex_y, ex_t ; % Prepare measurements T = ex_t ( - ex_t (1 ; % Determinde sample time %% Ex a Mobs = exsensor ( radar ; % Setup according to instructions Mobs.th = [, ]; Mobs.pe = diag ([1,.1].^ ; 1 %% Ex b X_tmp = zeros ( floor ( numel ( ex_t /1, ; P_tmp = zeros ( floor ( numel ( ex_t /1,, ; for i =1: ceil ( numel ( ex_t /1 % thmask must be set not to estimate the sensor position 1 tmp = estimate (Mobs, y (1*i -, thmask, [ ] ; X_tmp (i, : = tmp.x; P_tmp (i, :, : = cov ( tmp. px ; Xsnap = sig ( ex_y (1:1:, :,, [],... X_tmp, zeros ( numel ( ex_t (1:1:,,, P_tmp ; figure (1 ; clf ; % Plot the result xplot ( Xsnap, conf, ; print -depsc -f1 fig / exb ; %% Ex c M = exmotion ( cvd ; % We have no better information M = M. addsensor ( Mobs ; q = 1; % Tuning G = kron ([T ^/; T], eye ( ; Q = G*q.^* eye ( *G ; M. pv = Q; %% Ex d % Use the snapshot estimates to initialize x = [ Xsnap.x(1, :, ( Xsnap.x(, : -Xsnap.x(1, :/T] ; P = blkdiag (1* squeeze ( Xsnap.Px (1, :, :, 1* eye ( ; Xekf = ekf (M, y, x, x, P, P; figure ( ; clf ; % Plot the result xplot ( Xsnap, Xekf, conf, ; x 1 Y x1 (a Exercise (b X (b Exercise (c Figure 1: Figures for Exercise (b and (d.

3 . (a Start by computing all means and covariances (i, j = 1, E ȳ i = E(y i y = E(r i r + e i e = r i r cov(ȳ i, ȳ j = E ( (ȳ i E ȳ i (ȳ j E ȳ j = E ( (e i e (e j e = E ( { e i e j e i e e j e + e σ =, if i j σi + σ, if i = j where the last step uses that e i are mutually indepent. Hence, ( ( (ȳ1 r1 r E = (ȳ1 σ R = cov = 1 + σ σ ȳ r r ȳ σ σ + σ (b The given model is a nonlinear in x and with Gaussian noise, hence (. can be used to compute the Fisher information, I(x = H(x T R 1 H(x H(x = x h(x. Table.1 in the textbook (or derivation by hand yields, ( ( H(x = x 1 h 1 (x x h 1 (x = x P 1, y P,1 r y P, r x P x 1 h (x x h (x 1, r x P 1, y P, r r y P., r The CRLB can now be computed P crlb = I 1 (x = ( H(x T R 1 H(x 1 = ( x P 1, y P,1 r y P, r x P 1, r x P 1, r y P, r y P, r (c Given the values given, we get ( 1 1 H(x = 1 1 The CRLB is now: T (σ 1 + σ σ σ σ + σ 1 ( x P 1, y P,1 r y P, 1 r x P 1, r x P 1, r y P, r y P, r R = ( T ( 1 ( I(x = = P clrb = 1 ( 1 17 ( ( 17. 1

4 . load data17 %% Ex a % Calibrate the barometer model : y = C_1 * h+ C_ + e prs_cm = mean ( ex_ccm_prs ; prs_78cm = mean ( ex_c78cm_prs ; C = prs_cm ; C1 = ( prs_78cm - prs_cm /.78; % Estimate the measurement noise, cov ( e = R R = var ([ ex_ccm_prs - prs_cm ; ex_c78cm_prs - prs_78cm ]; 1 hhat = ( ex_prs - C/ C1; % Compute point estimates figure (1 ; clf ; % Plot the point estimates plot ( ex_t, hhat, x ; leg ( x^a_1 ; 1 %% Ex b, c % Assume a constant velocity height model exted with acc bias : % x = [ h; v; acc_bias ]; % h_k +1 = h_k + T*v + T ^/*( acc_y -g- bias + T ^/* w^v % v_k +1 = v_k + T*( acc_y -g- bias + T* w^v % biax_k +1 = bias_k + w^ b_k g =.8; % Gravity q =.1^; % Process noise, ie accelerometer noise. % Estimate the accelerometer bias based on the calibration sequences bias = mean ([ ex_ccm_acc ; ex_c78cm_acc ]; bias = bias ( - g; xsim = [; ; bias ]; % Results for b, x xhat = [; ; bias ]; % Results for c, x P = diag ([,.1, 1].^ ; % P H = [C1 ]; for k =: numel ( ex_t % Iterate over measurements T = ex_t ( k - ex_t (k -1 ; % Time step u = ex_acc (k -1, - g; % Input, based on accelerometer F = [1, T, -T ^/;, 1, -T;,, 1]; % F( T G = [T ^/; T; ]; % G(T Q = G*q*G ; Q(, = T *.1; % Q(T; % Time update xsim (:, k = F* xsim (:, k -1 + G*u; % b xhat (:, k = F* xhat (:, k -1 + G*u; % c P(:, :, k = F*P(:, :, k -1 *F + Q; % Measurement update if ~ isnan ( ex_prs (k yk = ex_prs ( k; % Measurement in meter S = H*P(:,:, k*h ; K = squeeze (P(:, :, k*h /S; xhat (:, k = xhat (:, k + K*(yk -H* xhat (:, k - C; P(:, :, k = ( eye ( - K*H* squeeze (P(:, :, k*( eye ( - K*H + K*R*K ; figure ( ; clf ; % Plot results Ex b plot ( ex_t, xsim ; leg ( x^b_1, x^b_, x^b_ ; figure ( ; clf ; % Plot results Ex c plot ( ex_t, hhat, x ; hold on; plot ( ex_t, xhat ; leg ( x^a_1, x^c_1, x^c_, x^c_ ; print -depsc -f1 fig / exa ; print -depsc -f fig / exb ; print -depsc -f fig / exc ;

5 1 8 x a 1 1 x b 1 x b x b (a Exercise (a (b Exercise (b. 1 x a 1 8 x c 1 x c x c (c Exercise (a, c Figure : Plots for exercise.