Solutions for examination in Sensor Fusion,

Similar documents
TSRT14: Sensor Fusion Lecture 8

TSRT14: Sensor Fusion Lecture 6. Kalman Filter (KF) Le 6: Kalman filter (KF), approximations (EKF, UKF) Lecture 5: summary

A Crash Course on Kalman Filtering

EXAMINATION IN TSRT14 SENSOR FUSION

Properties of Summation Operator

Nonlinear Estimation Techniques for Impact Point Prediction of Ballistic Targets

Kalman Filter. Predict: Update: x k k 1 = F k x k 1 k 1 + B k u k P k k 1 = F k P k 1 k 1 F T k + Q

1 Kalman Filter Introduction

Sensor Fusion, 2014 Lecture 1: 1 Lectures

Statistics 910, #15 1. Kalman Filter

Nonlinear Filtering. With Polynomial Chaos. Raktim Bhattacharya. Aerospace Engineering, Texas A&M University uq.tamu.edu

TSRT14: Sensor Fusion Lecture 1

Fisher Information Matrix-based Nonlinear System Conversion for State Estimation

A Comparison of the EKF, SPKF, and the Bayes Filter for Landmark-Based Localization

Variations. ECE 6540, Lecture 10 Maximum Likelihood Estimation

EE 565: Position, Navigation, and Timing

A new unscented Kalman filter with higher order moment-matching

Data assimilation with and without a model

Self-Driving Car ND - Sensor Fusion - Extended Kalman Filters

ML Estimation of Process Noise Variance in Dynamic Systems

Lecture 3: Functions of Symmetric Matrices

Lecture 7: Optimal Smoothing

FUNDAMENTAL FILTERING LIMITATIONS IN LINEAR NON-GAUSSIAN SYSTEMS

ECE531 Lecture 11: Dynamic Parameter Estimation: Kalman-Bucy Filter

The Kalman Filter ImPr Talk

DATA FUSION III: Estimation Theory

Sequential State Estimation (Crassidas and Junkins, Chapter 5)

Adaptive ensemble Kalman filtering of nonlinear systems

EEE 187: Take Home Test #2

Target tracking and classification for missile using interacting multiple model (IMM)

CS281A/Stat241A Lecture 17

Recursive Estimation

9 Multi-Model State Estimation

DESIGN AND IMPLEMENTATION OF SENSORLESS SPEED CONTROL FOR INDUCTION MOTOR DRIVE USING AN OPTIMIZED EXTENDED KALMAN FILTER

A Theoretical Overview on Kalman Filtering

6.4 Kalman Filter Equations

Lecture 8: Signal Detection and Noise Assumption

From Bayes to Extended Kalman Filter

IN particle filter (PF) applications, knowledge of the computational

4 Derivations of the Discrete-Time Kalman Filter

Linear Regression (9/11/13)

Data assimilation with and without a model

A Study of Covariances within Basic and Extended Kalman Filters

ECE 275A Homework 6 Solutions

System identification and sensor fusion in dynamical systems. Thomas Schön Division of Systems and Control, Uppsala University, Sweden.

(Extended) Kalman Filter

A Matrix Theoretic Derivation of the Kalman Filter

An Introduction to Signal Detection and Estimation - Second Edition Chapter III: Selected Solutions

Econ 2120: Section 2

ENGR352 Problem Set 02

Lecture 2: From Linear Regression to Kalman Filter and Beyond

Position Estimation and Modeling of a Flexible Industrial Robot

Sliding Window Test vs. Single Time Test for Track-to-Track Association

TSRT14: Sensor Fusion Lecture 9

Quaternion based Extended Kalman Filter

Technical report: Gaussian approximation for. superpositional sensors

Distributed estimation in sensor networks

Gaussian Mixtures Proposal Density in Particle Filter for Track-Before-Detect

Optimization-Based Control

RECURSIVE ESTIMATION AND KALMAN FILTERING

E190Q Lecture 11 Autonomous Robot Navigation

Ch 4. Linear Models for Classification

2-D SENSOR POSITION PERTURBATION ANALYSIS: EQUIVALENCE TO AWGN ON ARRAY OUTPUTS. Volkan Cevher, James H. McClellan

Data Fusion Kalman Filtering Self Localization

RAO-BLACKWELLISED PARTICLE FILTERS: EXAMPLES OF APPLICATIONS

STAT 100C: Linear models

State Estimation using Moving Horizon Estimation and Particle Filtering

ECE276A: Sensing & Estimation in Robotics Lecture 10: Gaussian Mixture and Particle Filtering

On Underweighting Nonlinear Measurements

EE363 homework 5 solutions

557: MATHEMATICAL STATISTICS II BIAS AND VARIANCE

Advanced Signal Processing Minimum Variance Unbiased Estimation (MVU)

Extension of Farrenkopf Steady-State Solutions with Estimated Angular Rate

Rao-Blackwellized Particle Filtering for 6-DOF Estimation of Attitude and Position via GPS and Inertial Sensors

A Few Notes on Fisher Information (WIP)

Proof In the CR proof. and

Estimation Theory Fredrik Rusek. Chapters

Joint GPS and Vision Estimation Using an Adaptive Filter

Lecture 4: Proofs for Expectation, Variance, and Covariance Formula

Extended Kalman Filter Tutorial

Kalman-Filter-Based Time-Varying Parameter Estimation via Retrospective Optimization of the Process Noise Covariance

Principles of forecasting

1 Introduction ISSN

Conditional Posterior Cramér-Rao Lower Bounds for Nonlinear Sequential Bayesian Estimation

Lecture Note 1: Probability Theory and Statistics

COS Lecture 16 Autonomous Robot Navigation

Randomized Unscented Kalman Filter in Target Tracking

A Comparitive Study Of Kalman Filter, Extended Kalman Filter And Unscented Kalman Filter For Harmonic Analysis Of The Non-Stationary Signals

SIMULTANEOUS STATE AND PARAMETER ESTIMATION USING KALMAN FILTERS

Information Theory for Wireless Communications, Part II:

Statement: With my signature I confirm that the solutions are the product of my own work. Name: Signature:.

Posterior Cramer-Rao Lower Bound for Mobile Tracking in Mixed Line-of-Sight/Non Line-of-Sight Conditions

Introduction to Unscented Kalman Filter

Lecture Notes 4 Vector Detection and Estimation. Vector Detection Reconstruction Problem Detection for Vector AGN Channel

L06. LINEAR KALMAN FILTERS. NA568 Mobile Robotics: Methods & Algorithms

Using the Kalman Filter to Estimate the State of a Maneuvering Aircraft

Infinite Horizon LQ. Given continuous-time state equation. Find the control function u(t) to minimize

Least Squares Estimation Namrata Vaswani,

Modeling and Analysis of Dynamic Systems

Statistical Filtering and Control for AI and Robotics. Part II. Linear methods for regression & Kalman filtering

Transcription:

Solutions for examination in Sensor Fusion, 17-- 1. (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 1 1 + 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

. 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, ; 1. 1. 1 1. 1. 18 1 1. 1 1. x 1 Y 1 8 8 7. 1. 1 7. 1 7 1. 1 - - - 1 1 x1 (a Exercise (b - 1 1 X (b Exercise (c Figure 1: Figures for Exercise (b and (d.

. (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 ( 1 1 1 1 1 I(x = = 1 1 1 1 1 1 1 P clrb = 1 ( 1 17 ( 1 1 1 ( 17. 1

. 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 ;

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

2024 © sciencedocbox.com Privacy Policy | Terms of Service | Feedback