Adaptive Filtering and Change Detection Fredrik Gustafsson Copyright © 2000 John Wiley & Sons, Ltd ISBNs: 0-471-49287-6 (Hardback); 0-470-84161-3 (Electronic) Part W : State estimation Adaptive Filtering and Change Detection Fredrik Gustafsson Copyright © 2000 John Wiley & Sons, Ltd ISBNs: 0-471-49287-6 (Hardback); 0-470-84161-3 (Electronic) Kalman filtering 8.1 Basics 264 8.2 State spacemodeling 267 8.2.1 Samplingformula 268 modeling 268 8.2.2 Physical 8.2.3 Using known transferfunctions 272 274 8.2.4 Modeling tricks 8.3.The Kalman filter 278 8.3.1 Basic formulas 278 8.3.2 Numerical examples 280 properties 284 8.3.3 Optimality 8.4 Time-invariant signal model 286 8.4.1 Error sources 287 8.4.2 Observer 288 8.4.3 Frequency response 289 289 8.4.4 Spectral factorization 8.5 Smoothing 290 8.5.1 Fixed-lag smoothing 290 8.5.2 Fixed-interval smoothing 292 8.6 Computational aspects 295 8.6.1 Divergence 296 297 8.6.2 Crosscorrelated noise 8.6.3 Bias error 298 299 8.6.4 Sequential processing 8.7 Square rootimplementation 300 8.7.1 Timeand measurement updates 301 304 8.7.2 Kalman predictor 8.7.3 Kalman filter 304 8.8 Sensorfusion 306 8.8.1 The informationfilter 308 8.8.2 Centralized fusion 310 8.8.3 The general fusion formula 310 311 8.8.4 Decentralized fusion 8.9 The extended Kalman filter 313 Kalman filterina 264 8.9.1 Measurement update 313 8.9.2 Time update 314 318 8.9.3 Linearization error 8.9.4.Discretization of state noise 321 8.10 Whiteness based change detection using the Kalman filter 324 8.11 Estimation of covariances in state space models 326 8.12 Applications 327 8.12.1 DC motor 327 8.12.2 Targettracking 328 337 8.12.3 GPS 8.1 Basics The goal in this section is to explain the fundamentalsof Kalman filter theory by a few illustrative examples The Kalman filter requires a state space model for describing the signal dynamics To describe its role, we need a concrete example, so let us return to the target tracking example from Chapter Assume that we want a model with the states z1= X , x2 = Y , x3 = X och x4 = Y This is the simplest possible case of state vector used in practice Before we derive a model in the next section, a few remarks will be given on what role the different terms in the model has Example 8.7 Target tracking: function of Kalman filter Figure 8.l(a) illustrates how the Kalman filter makes use of the model Suppose the object in a target tracking problem (exactly the same reasoning holds for navigation as well) is located at the origin with velocity vector (1,l) For simplicity, assume that we have an estimate of the state vector which coincides with the true value &l0 = (O,O, , l )T = xi In practice, there will of course be some errororuncertaintyin the estimate The uncertainty can be described by a confidence interval, which in the Kalman filter approach is always shaped as anellipsoid In the figure, the uncertainty is larger in the longitudinal direction than in the lateral direction Given the initial state, future positions can be predicted by just integrating the velocity vector (like in dead reckoning) This simulation yields a straight line for the position With a more complex state space model with morestates, more complex manoeuvres can be simulated In the state noise description, 8.1 Basics 265 -1 0.5 (a) 1.5 2.5 3.5 -I (b) Figure 8.1 The plot in (a) shows how the model predicts future positions of the object, given the state vector at time 0, and how the uncertainty region increases in time The plot in (b) shows how the filter takes care of the first measurement by correctingthe state vector and decreasing the uncertainty region we can model what kind of manoeuvres can be performed by the aircraft, for instance, m/s2 in longitudinal direction and m/s2 in lateral direction The possible and unpredictable manoeuvres in the future imply that the uncertainty grows in time, just like illustrated by the ellipsoids in Figure 8.l(a) Note that the object makes a very sharp manoeuvre that makes the actual path xt going outside the confidence interval Figure 8.l(b) shows how the Kalmanfilter acts when the first measurement x1 becomes available First, the estimate is corrected towards the measurement The velocity state component is adapted likewise Then the uncertainty is decreased accordingly The index rules can now be summarized as: 0 For the true trajectory xt,a single index is used as usual Time is indeed continuous here For a simulation starting at time 0, as shown in Figure S.l(a), the se~ , is used The rule is that the firstindex is quence xol0,x1l0, time and the second one indicates when the simulation is started When the Kalman filter updates the estimate, the second index is increased one unit The Kalman filter is alternating between a one time step simulation, and updating the statevector, which yields the sequence xOIO, x l l O , x l l l , x211, x212, 2312, * Kalman filterina 266 Hats are used on all computed quantities (simulated or estimated), instance Q t for The state space model used in this chapter is Here yt is a measured signal, A, B , C,D are known matrices and xt an unknown state vector There are three inputs to the system: the observable (and controllable) ut, the non-observable process noise vt and the measurement noise et The Kalman filter is given by where the update gain Kt is computed by the the Kalman filter equations as a function Figure 8.2 summarizes the signal flow for the signal model and the Kalman filter Noises e t , wt Input ut c c System o u t p u t yt State xt Yt c Ut Kalman filter Et M et Pt M 2t c Figure 8.2 Definition of signals for the signal model and Kalman filter Example 8.2 Target tracking: function of state space model The role of the statespace model in Example8.1 is summarized as follows: The deterministic model (the A matrix) describes how to simulate the state vector In the example, the velocity state is used to find future positions 8.2 State space modeling 267 The stochastic term in the state equation, &ut, decides how the confidence interval grows (the size of the random walk) One extreme case is Q + CO,when anything can happen in one time step and all old information must be considered as useless, and the ellipsoid becomes infinitely large The other extreme case is Q + 0, which corresponds to the ellipsoid not growing at all The deterministic part of the measurement equation yt = Cxt tells the Kalman filter in what direction one measurement should affect the estimate The covariance R of the measurement noise et describes how reliable the measurement information is Again, the extreme points are important to understand First, R = says that the measurement is exact, and in the example this would imply that illlwould coincide with x1, and the corresponding ellipsoid must break down to a point Secondly, R + m means that the measurement is useless and should be discarded Much of the advantage of using the Kalman filter compared to more ad hoc filters (low-pass) is that the design is moved from an abstract pole-zero placement to a more concrete level of model design Literature The standard reference for all computational aspects of the Kalman filter has for a long time been Anderson and Moore (1979), but from now on it is likely that the complete reference will be Kailath et al (1998) This is a thorough work covering everything related to state space estimation These book with two references are a bit weak regarding applications A suitable navigation applications is Minkler and Minkler (1990) Other monographs are Brown and Hwang (1997) and Chui and Chen (1987) 8.2 State spacemodeling Designing a good Kalman filter is indeed more a modeling than a filter design task All that is needed to derive a good Kalman filter in an application is to understand the modeling and a few tuning issues The rest is implementation problems that can be hidden in good software That is, the first sub-goal is to derive a state space model Kalman filterina 268 The model is completely specified by the matrices A , B = [B,, B,], C,D ,Q , R System modeling is a subject coveredby many textbooks; see for instance Ljung and Glad (1996) The purpose here is to give a few but representative examples to cover the applications in this part Since many models are derived in continuous time, but we are concerned with discrete time filtering, we start by reviewing some sampling formulas 8.2.1.Samplingformula Integration over one sample period T of the continuous time model gives Xt+T =xt + (ACx,+ B:u7) d r Assuming that the deterministic input is constant during thesampling interval, which is the case in for instance computer controlled systems, thesolution can be written as the discrete time state space model xt+T = Axt + &ut7 where A =e ACT The same sampling formulas can be used for the stochastic input ut in (8.4), although it is seldom the case that ut is constant during thesampling intervals Other alternatives are surveyed in Section 8.9 There are three main ways to compute the matrixexponential: using the Laplace transform,series expansion or algebraic equations See, for example, Astrom and Wittenmark (1984) for details Standard computer packages for control theory can be used, like the function c2d in Control System Toolbox in MATLABTM 8.2.2.Physicalmodeling Basic relations from different disciplines in form of differential equations can be used to build up a state space model The example below illustrates many aspects of how the signal processing problem can be seen as a modeling one, rather than a pure filter design Much can be gained in performance by designing the model appropriately 8.2 State mace modelina 269 Figure 8.3 Trackingcoordinates Example 8.3 Target tracking: modeling Consider an aircraft moving in two dimensions as in Figure 8.3 Using Newton’s law F = m u separately in two dimensions gives =-F xt m =- F2 xt m Here Fi are forces acting on the aircraft, normally due to the pilot’s manoeuvres, which are unknown to the tracker From now on, the right-hand side will be considered as random components W: In vector form, we get The standard form of a state space model only has first order derivatives in the left-hand side We therefore introduce dummy states, which here has the physical interpretation of velocities: 2; =X; xt” =xt xt” =wt1 - -w2 xt - t‘ Here the noise is denoted wirather than v so as not to confuse it with the velocities vi = xi, i = 1’2 Assuming that the position is measurable, we get a continuous time state space model for the state vector X = (xl, x2,v’, w2) in standard form: Kalman filterina 270 ( 00 00 &=ACzt+B3ut= 0 )G+ (;1) Wt, 0 0 Yt = 0 o) xt +et (o Here superscript c indicates continuous time The sample formula with sampling interval T gives There are two possibilities for the choice of Q The simplest is a diagonal Q = 412 An aircraft can accelerate much faster orthogonally to the velocity vector than parallel to it Thatis, the forces during turns aremuch larger than during acceleration and retardation This implies that we would like to have different adaptation gains in these two directions The only problem is that the aircraft has its own coordinate system which is not suitable for tracking However, we can use the heading (velocity direction) estimate and assume that the velocity changes mainlyorthogonally to h It can be shown that the appropriate state dependent covariance matrix is Q= ( + qw sin2(h) qv cos2(h) (qv - q,,) sin(h) cos(h) (qv - qw) sin(h)cos(h) qw cos2( h ) qv sin2( h ) + ) (8.8) Here qv is the force variance along the velocity vector and qw is perpendicular to it, where qw >> qv It has been pointed out by many authors that during manoeuvres, better tracking can be achieved by a so-called jerk model Here two more acceleration states are included in the model, and the state noise is now the jerkratherthan acceleration The state space model for the state vector IC = (X', x ,v', w2, U', u2)T is: 8.2 State mace modelina 271 :1 ‘0 0 0 ~ O O l O O O l O O O 0 \ 0 T T O l 0 0 O O T O O T 0 (i T3/3 xt + Wt wt Again, the covariance matrix in (8.8) is a reasonable option Another strategy is a compromise between the acceleration and jerk models, inspired by the physical constraints of the motion of the aircraft Since the acceleration is mainly orthogonal to thevelocity vector, one more state can be introduced for this acceleration The acceleration orthogonal to thevelocity is , v ,w ) ~ the turn rate W , and the state equationsfor the statevector ( x l , x vl, is given by: -1 xt =ut1 -2 xt =ut2 -1 ut = - WtVt2 -2 ut =W& (8.11) W, =o The main advantage of this model is that circular paths correspond to cona stant turn rate, so we have a parameter estimation problem rather than trackingproblem in these segments Paths consisting of straight lines and circle segments are called coordinated turns in the literature of Air Trufic Control (ATC) One further alternative is to use velocities in polar coordinates rather than ,h, w ) ~the , statedynamics become: Cartesian With the statevector ( x l , x v, .. .Adaptive Filtering and Change Detection Fredrik Gustafsson Copyright © 2000 John Wiley & Sons, Ltd... innovations The explanation is the prior belief in the model More on this in Section 8.6.1 As for adaptive filters, it is very illuminating to analyze the estimation error in terms of bias, variance