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

Transcription

1 TSRT14 Lecture 6 Gustaf Hendeby Spring / 42 Le 6: Kalman filter KF approximations EKF UKF TSRT14: Sensor Fusion Lecture 6 Kalman filter KF KF approximations EKF UKF Gustaf Hendeby hendeby@isyliuse Whiteboard: ˆ Derivation framework for KF EKF UKF Slides: ˆ Kalman filter summary: main equations robustness sensitivity divergence monitoring user aspects ˆ Nonlinear transforms revisited ˆ Application to derivation of EKF and UKF ˆ User guidelines and interpretations TSRT14 Lecture 6 Gustaf Hendeby Spring / 42 Lecture 5: summary ˆ Standard models in global coordinates: ˆ Translation p m t = w p t ˆ 2D orientation for heading h m t = wt h ˆ Coordinated turn model Ẋ = v X v X = ωv Y ω = Ẏ = v Y ˆ Standard models in local coordinates x y ψ ˆ Odometry and dead reckoning for x y ψ v Y = ωv X Kalman Filter KF t X t = X + vt x cosψt dt Yt = Y + t t vt x sinψt dt ψ t = ψ + ψ t dt ˆ Force models for ψ a y a x ˆ 3D orientation q = 1 2 Sωq = 1 Sqω 2

2 TSRT14 Lecture 6 Gustaf Hendeby Spring / 42 Chapter 7 Overview Kalman filter ˆ Algorithms and derivation ˆ Practical issues ˆ Computational aspects ˆ Filter monitoring The discussion and conclusions do usually apply to all nonlinear filters though it is more concrete in the linear Gaussian case TSRT14 Lecture 6 Gustaf Hendeby Spring / 42 Kalman Filter KF Time-varying state space model: x k+1 = F k x k + G k v k covv k = Q k y k = H k x k + e k cove k = R k Time update: ˆx k+1 k = F k ˆx k k P k+1 k = F k P k k Fk T + G kq k G T k Measurement update: ˆx k k = ˆx k k 1 + P k k 1 Hk T H kp k k 1 Hk T + R k 1 y k H k ˆx k k 1 P k k = P k k 1 P k k 1 Hk T H kp k k 1 Hk T + R k 1 H k P k k 1 TSRT14 Lecture 6 Gustaf Hendeby Spring / 42 KF Modifications Auxiliary quantities: innovation ε k innovation covariance S k and Kalman gain K k ŷ k = H k ˆx k k 1 TSRT14 Lecture 6 Gustaf Hendeby Spring / 42 Simulation Example 1/2 Create a constant velocity model simulate and Kalman filter ε k = y k H k ˆx k k 1 = y k ŷ k S k = H k P k k 1 H T k + R k K k = P k k 1 H T k H kp k k 1 H T k + R k 1 = P k k 1 H T k S 1 k Filter form Predictor form ˆx k k = F k 1ˆx k 1 k 1 + K k y k H k F k 1ˆx k 1 k 1 = F k 1 K k H k F k 1 ˆx k 1 k 1 + K k y k T = 5; F = [1 T ; 1 T; 1 ; 1]; G = [T ^2/2 ; T ^2/2; T ; T]; H = [1 ; 1 ]; R = 3* eye 2 ; m = lss F [] H [] G*G R 1/T; m xlabel = { X Y vx vy }; m ylabel = { X Y }; m name = Constant velocity motion model ; z = simulate m 2 ; xhat1 = kalman m z alg 2 k 1; % Time - varying xplot2 z xhat1 conf 9 [1 2] ; Y X ˆx k+1 k = F k ˆx k k 1 + F k K k y k H k ˆx k k 1 = F k F k K k H k ˆx k k 1 + F k K k y k

3 TSRT14 Lecture 6 Gustaf Hendeby Spring / 42 Simulation Example 2/2 Covariance illustrated as confidence ellipsoids in 2D plots or confidence bands in 1D plots xplot z xhat1 conf 99 y1 y2 y3 y Time Time Time Time TSRT14 Lecture 6 Gustaf Hendeby Spring / 42 Tuning the KF ˆ The SNR ratio Q / R is the most crucial it sets the filter speeds Note difference of real system and model used in the KF ˆ Recommentation: fix R according to sensor specification/performance and tune Q motion models are anyway subjective approximation of reality ˆ High SNR in the model gives fast filter that is quick in adapting to changes/maneuvers but with larger uncertainty small bias large variance ˆ Conversely low SNR in the model gives slow filter that is slow in adapting to changes/maneuvers but with small uncertainty large bias small variance ˆ P reflects the belief in the prior x 1 N ˆx 1 P Possible to choose P very large and ˆx 1 arbitrary if no prior information exists ˆ Tune covariances in large steps order of magnitudes! TSRT14 Lecture 6 Gustaf Hendeby Spring / 42 Optimality Properties TSRT14 Lecture 6 Gustaf Hendeby Spring / 42 Robustness and Sensitivity ˆ For a linear model the KF provides the WLS solution ˆ The KF is the best linear unbiased estimator BLUE ˆ It is the Bayes optimal filter for linear model when x v k e k are Gaussian variables x k+1 y 1:k N ˆx k+1 k P k+1 k x k y 1:k N ˆx k k P k k ε k N S k The following concepts are relevant for all filtering applications but they are most explicit for KF: ˆ Observability is revealed indirectly by P k k ; monitor its rank or better condition number ˆ Divergence tests Monitor performance measures and restart the filter after divergence ˆ Outlier rejection monitor sensor observations ˆ Bias error incorrect model gives bias in estimates ˆ Sensitivity analysis uncertain model contributes to the total covariance ˆ Numerical issues may give complex estimates

4 TSRT14 Lecture 6 Gustaf Hendeby Spring / 42 Observability 1 Snapshot observability if H k has full rank WLS can be applied to estimate x 2 Classical observability for time-invariant and time/varying case H H k n+1 HF H k n+2 F k n+1 HF 2 O = HF n 1 O k = H k n+3 F k n+2 F k n+1 H k F k 1 F k n+1 3 The covariance matrix P k k extends the observability condition by weighting with the measurement noise and to forget old information according to the process noise Thus the condition number of P k k is the natural indicator of observability! TSRT14 Lecture 6 Gustaf Hendeby Spring / 42 Divergence Tests When is ε k ε T k significantly larger than its computed expected value S k = Eε k ε T k note that ε k N S k? Principal reasons: ˆ Model errors ˆ Sensor model errors: offsets drifts incorrect covariances scaling factor in all covariances ˆ Sensor errors: outliers missing data ˆ Numerical issues Solutions: ˆ In the first two cases the filter has to be redesigned ˆ In the last two cases the filter has to be restarted TSRT14 Lecture 6 Gustaf Hendeby Spring / 42 Outlier Rejection Let H : ε k N S k then T y k = ε T k S 1 k ε k χ 2 n yk if everything works fine and there is no outlier If T y k > h PF A this is an indication of outlier and the measurement update can be omitted In the case of several sensors each sensor i should be monitored for outliers T y i k = εi k T S 1 k εi k χ2 n y i k TSRT14 Lecture 6 Gustaf Hendeby Spring / 42 Sensitivity analysis: parameter uncertainty Sensitivity analysis can be done with respect to uncertain parameters with known covariance matrix using for instance Gauss approximation formula ˆ Assume F θ Gθ Hθ Qθ Rθ have uncertain parameters θ with Eθ = ˆθ and covθ = P θ ˆ The state estimate ˆx k is a stochastic variable with four stochastic sources v k e k x 1 at one hand and θ on the other hand ˆ The law of total variance varx = E varx Y + var EX Y and Gauss approximation formula varhy h Y Ȳ vary h Y Ȳ T gives covˆx k k P k k + dˆx k k dθ P θ dˆxk k dθ T ˆ The gradient dˆx k k /dθ can be computed numerically by simulations

5 TSRT14 Lecture 6 Gustaf Hendeby Spring / 42 Numerical Issues Some simple fixes if problem occurs: ˆ Assure that the covariance matrix is symmetric P = 5*P + 5*P ˆ Use the more numerically stable Joseph s form for the measurement update of the covariance matrix: P k k = I K k H k P k k 1 I K k H k T + K k R k K T k ˆ Assure that the covariance matrix is positive definite by setting negative eigenvalues in P to zero or small positive values ˆ Avoid singular R = even for constraints ˆ Dithering Increase Q and R if needed; this can account for all kind of model errors Kalman Filter Approximations EKF UKF TSRT14 Lecture 6 Gustaf Hendeby Spring / 42 Chapter 8 Overview TSRT14 Lecture 6 Gustaf Hendeby Spring / 42 EKF1 and EKF2 principle ˆ Nonlinear transformations ˆ Details of the EKF algorithms ˆ Numerical methods to compute Jacobian and Hessian in the Taylor expansion ˆ An alternative EKF version without the Ricatti equation ˆ The unscented Kalman filter UKF Apply TT1 and TT2 respectively to the dynamic and observation models For instance x k+1 = fx k + v k = fˆx + g ˆxx ˆx x ˆxT g ξx ˆx ˆ EKF1 neglects the rest term ˆ EKF2 compensates with the mean and covariance of the rest term using ξ = ˆx

6 TSRT14 Lecture 6 Gustaf Hendeby Spring / 42 EKF1 and EKF2 Algorithm TSRT14 Lecture 6 Gustaf Hendeby Spring / 42 Comments NB! S k = h xˆx k k 1 P k k 1 h xˆx k k 1 T + h eˆx k k 1 R k h eˆx k k 1 T + 1 [ 2 trh ix ˆx k k 1P k k 1 h jx ˆx k k 1P k k 1 ] ij K k = P k k 1 h x ˆx k k 1 T S 1 k ε k = y k hˆx k k 1 1 [ 2 trh ix P k k 1 ] i ˆx k k = ˆx k k 1 + K k ε k P k k = P k k 1 P k k 1 h xˆx k k 1 T S 1 k h x ˆx k k 1P k k [ 2 trh ix ˆx k k 1P k k 1 h jx ˆx k k 1P k k 1 ] ij ˆx k+1 k = fˆx k k + 1 [ 2 trf ix P k k ] i P k+1 k = f x ˆx k kp k k f x ˆx k k T + f v ˆx k kq k f v ˆx k k T + 1 [ 2 trf ix ˆx k kp k k f jx ˆx k kp k k ] ij This form of the EKF2 as given in the book is disregarding second order terms of the process noise! See eg my thesis for the full expressions ˆ The EKF1 using the TT1 transformation is obtained by letting both Hessians f x and h x be zero ˆ Analytic Jacobian and Hessian needed If not available use numerical approximations done in Signal and Systems Lab by default! ˆ The complexity of EKF1 is as in KF n 3 x due to the F P F T operation ˆ The complexity of EKF2 is n 5 x due to the F i P Fj T i j = 1 n x operation for ˆ Dithering is good! That is increase Q and R from the simulated values to account for the approximation errors TSRT14 Lecture 6 Gustaf Hendeby Spring / 42 EKF Variants TSRT14 Lecture 6 Gustaf Hendeby Spring / 42 Derivative-Free Algorithms ˆ The standard EKF linearizes around the current state estimate ˆ The linearized Kalman filter linearizes around some reference trajectory ˆ The error state Kalman filter also known as the complementary Kalman filter estimates the state error x k = x k ˆx k with respect to some approximate or reference trajectory Feedforward or feedback configurations linearized Kalman filter = feedforward error state Kalman filter EKF = feedback error state Kalman filter Numeric derivatives are preferred in the following cases: ˆ The nonlinear function is too complex ˆ The derivatives are too complex functions ˆ A user-friendly algorithm is desired with as few user inputs as possible This can be achieved with either numerical approximation or using sigma points!

7 TSRT14 Lecture 6 Gustaf Hendeby Spring / 42 Nonlinear transformations NLT Consider a second order Taylor expansion of a function z = gx: z = gx = gˆx + g ˆxx ˆx x ˆxT g ξx ˆx }{{} rx;ˆxg ξ The rest term is negligible and EKF works fine if: ˆ the model is almost linear ˆ or the SNR is high so x ˆx can be considered small The size of the rest term can be approximated a priori Note: the size may depend on the choice of state coordinates! If the rest term is large use either of ˆ the second order compensated EKF that compensates for the mean and covariance of rx; ˆx g ξ rx; ˆx g ˆx ˆ the unscented KF UKF TSRT14 Lecture 6 Gustaf Hendeby Spring / 42 TT1: first order Taylor approximation The first order Taylor term gives a contribution to the covariance: x N ˆx P N gˆx [g iˆxp g jˆx T ] ij = N gˆx g ˆxP g ˆx T ˆ This is sometimes called Gauss approximation formula ˆ Here [A] ij means element i j in the matrix A This is used in EKF1 EKF with first order Taylor expansion Leads to a KF where nonlinear functions are approximated with their Jacobians ˆ Compare with the linear transformation rule z = Gx x N ˆx P z N Gˆx GP G T ˆ Note that GP G T can be written [G i P G T j ] ij where G i denotes row i of G TSRT14 Lecture 6 Gustaf Hendeby Spring / 42 TT2: second order Taylor approximation TSRT14 Lecture 6 Gustaf Hendeby Spring / 42 MC: Monte Carlo sampling The second order Taylor term contributes both to the mean and covariance as follows: x N ˆx P N gˆx+ 1 2 [trg i ˆxP ] i [g iˆxp g jˆx T trp g i ˆxP g j ˆx] ij ˆ This is used in EKF2 EKF with second order Taylor expansion Leads to a KF where nonlinear functions are approximated with their Jacobians and Hessians ˆ UKF tries to do this approximation numerically without forming the [ Hessian g x explicitly This reduces the n 5 x complexity in tr P g i ˆxP g j ˆx] ij to n3 x complexity Generate N samples transform them and fit a Gaussian distribution x i N ˆx P z i = gx i µ z = 1 N N i=1 P z = 1 N 1 z i N z i µ z z i T µ z i=1 Not commonly used in nonlinear filtering but a valid and solid approach!

8 TSRT14 Lecture 6 Gustaf Hendeby Spring / 42 UT: the unscented transform At first sight similar to MC: Generate 2n x + 1 sigma points transform these and fit a Gaussian distribution: x = ˆx x ±i = ˆx ± n x + λp 1/2 :i i = 1 2 n x z i = gx i n x λ 1 Ez 2n x + λ z + 2n i= n x + λ zi x λ z covz 2n x + λ +1 α2 + β Ez z Ez T + n x i= n x 1 2n x + λ z i Ez z i Ez T TSRT14 Lecture 6 Gustaf Hendeby Spring / 42 UT: design parameters ˆ λ is defined by λ = α 2 n x + κ n x ˆ α controls the spread of the sigma points and is suggested to be chosen around 1 3 ˆ β compensates for the distribution and should be chosen to β = 2 for Gaussian distributions ˆ κ is usually chosen to zero Note ˆ n x + λ = α 2 n x when κ = ˆ The weights sum to one for the mean but sum to 2 α 2 + β 4 for the covariance Note also that the weights are not in [ 1] ˆ The mean has a large negative weight! ˆ If n x + λ then UT and TT2 and hence UKF and EKF2 are identical for n x = 1 otherwise closely related! TSRT14 Lecture 6 Gustaf Hendeby Spring / 42 Example 1: squared norm Squared norm of a Gaussian vector has a known distribution: z = gx = x T x x N I n z χ 2 n Theoretical distribution is χ 2 n with mean n and variance 2n The mean and variance are below summarized as a Gaussian distribution The number of Monte Carlo simulations is 1 n TT1 TT2 UT1 UT2 MCT 1 N N 1 2 N 1 2 N 1 2 N N N 2 4 N 2 2 N 2 8 N N N 3 6 N 3 N 3 18 N N N 4 8 N 4 4 N 4 32 N N N 5 1 N 5 1 N 5 5 N Theory N N n 2n N n 3 nn N n 2n 2 N n 2n Conclusion: TT2 works not the unscented transforms TSRT14 Lecture 6 Gustaf Hendeby Spring / 42 Example 2: radar Conversion of polar measurements to Cartesian position: x1 cosx z = gx = 2 x 1 sinx 2 X TT1 TT UT1 UT2 MCT Conclusion: UT works better than TT1 and TT2

9 TSRT14 Lecture 6 Gustaf Hendeby Spring / 42 Example 3: standard sensor networks measurements Standard measurements: n g toa x = x = i=1 x2 i TOA 2D: gx = x X N [3; ] [1 ; 1] TT1 N 3 1 TT2 N UT2 N MCT N g doa x = arctan2x 1 x 2 DOA: gx = arctan2x 2 x 1 X N [3; ] [1 ; 1] TT1 N 111 TT2 N 235 UT2 N MCT N Conclusion: UT works slightly better than TT1 and TT2 Studying RSS measurements g rss x = c c 2 1 log 1 x 2 TSRT14 Lecture 6 Gustaf Hendeby Spring / 42 KF EKF and UKF in one framework Lemma 71 If X N Y µx µ y Pxx P xy = N P xy P yy µx µ y P Then the conditional distribution for X given the observed Y = y is Gaussian distributed: X Y = y N µ x + P xy P 1 yy y µ y P xx P xy P 1 yy P yx Connection to the Kalman filter The Kalman gain is in this notation given by K k = P xy P 1 yy gives similar results TSRT14 Lecture 6 Gustaf Hendeby Spring / 42 Kalman Filter Algorithm 1/2 Time update: Let x = xk v k N ˆxk k Pk k Q k z = x k+1 = fx k u k v k = g x The transformation approximation UT MC TT1 TT2 gives z N ˆx k+1 k P k+1 k TSRT14 Lecture 6 Gustaf Hendeby Spring / 42 Kalman Filter Algorithm 2/2 Measurement update: Let xk x = N e k xk z = = y k ˆxk k 1 x k hx k u k e k ŷ k k 1 Pk k 1 R k = g x The transformation approximation UT MC TT1 TT2 gives ˆxk k 1 P xx k k 1 P xy k k 1 z N The measurement update is now K k = P xy k k 1 P yy P yx k k 1 k k 1 1 P yy k k 1 ˆx k k = ˆx k k 1 + K k yk ŷ k k 1

10 TSRT14 Lecture 6 Gustaf Hendeby Spring / 42 Comments TSRT14 Lecture 6 37 / 42 Depends mainly on: i SNR ii the degree of nonlinearity iii the degree of non-gaussian noise in particular if any distribution is multi-modal has several local maxima SNR and degree of nonlinearity is connected through the rest EKF1 The filter obtained using TT2 is equivalent to EKF2 term whose expected value is: Ex x T g ξx x = E tr g ξx x x x T The filter obtained using UT is equivalent to UKF The Monte Carlo approach should be the most accurate since it asymptotically computes the correct first and second order moments There is a freedom to mix transform approximations in the time and measurement update Gustaf Hendeby Spring / 42 Virtual Yaw Rate Sensor Small rest term requires either high SNR small P or almost linear functions small f and h If the rest term is small use EKF1 If the rest term is large and the nonlinearities are essentially quadratic example xt x use EKF2 If the rest term is large and the nonlinearities are not essentially quadratic try UKF If the functions are severly nonlinear or any distribution is multi-modal consider filterbanks or particle filter TSRT14 Lecture 6 Gustaf Hendeby Spring / 42 Navigation grade IMU gives accurate dead-reckoning but drift may cause return at bad places GPS is restricted for high speeds and high accelerations Fusion of IMU and GPS when pseudo-ranges are available with IMU support to tracking loops inside GPS distance rk3 rk4 and the measurements ω3 rnom + ω4 rnom 2 = 2 B ω3 ω4 ω3 ω4 rk3 rk4 rk3 rk Loose integration: direct fusion approach yk = pk + ek Tight integration: TDOA fusion approach yki = pk pik /c + tk + ek + e2k Model with state vector xk = ψ k ψ k bk yk2 = tr g ξp Sounding Rocket Yaw rate subject to bias orientation error increases linearly in time Wheel speeds also give a gyro where the orientation error grows linearly in e1k TSRT14 Lecture 6 = ψ k + bk + Spring 217 Choice of Nonlinear Filter The filter obtained using TT1 is equivalent to the standard yk1 Gustaf Hendeby

11 TSRT14 Lecture 6 Gustaf Hendeby Spring / 42 Sounding Rocket Navigation grade IMU gives accurate dead-reckoning but drift may cause return at bad places GPS is restricted for high speeds and high accelerations Fusion of IMU and GPS when pseudo-ranges are available with IMU support to tracking loops inside GPS ˆ Loose integration: direct fusion approach y k = p k + e k ˆ Tight integration: TDOA fusion approach y i k = p k p i k /c + t k + e k Error [m] Error [m] x y z Position error relative reference trajectory Time [s] Position error relative reference trajectory Time [s] x y z TSRT14 Lecture 6 Gustaf Hendeby Spring / 42 MC Leaning Angle ˆ Headlight steering ABS and anti-spin systems require leaning angle ˆ Gyro very expensive for this application ˆ Combination of accelerometers investigated lateral and downward acc worked fine in EKF Model where z y z z a 1 a 2 J are constants relating to geometry and inertias of the motorcycle u = v x x = ϕ ϕ ϕ ψ ψ δay δ az δ ϕ T y = hx = ay ux 4 z yx 3 + z yx 2 4 tanx 1 + g sinx 1 + x 6 a ż = ux 4 tanx 1 z z x x 2 4 tan2 x 1 + g cosx 1 + x 7 ϕ a 1 x 3 + a 2 x 2 4 tanx 1 ux 4 J + x 6 TSRT14 Lecture 6 Gustaf Hendeby Spring / 42 Summary Lecture 6 Summary Key tool for a unified derivation of KF EKF UKF X µx Pxx P N xy Y µ y P xy P yy X Y = y N µ x + P xyp 1 1 yy y µy Pxx PxyP yy Pyx The Kalman gain is in this notation given by K k = P xy P 1 yy ˆ In KF P xy and P yy follow from a linear Gaussian model ˆ In EKF P xy and P yy can be computed from a linearized model requires analytic gradients ˆ In EKF and UKF P xy and P yy computed by NLT for transformation of x T v T T and x T e T T respectively No gradients required just function evaluations