M & C
Morgan
ol & C l a y p o ol
Publishers
An Introduct Introduction ion to Kalman Filtering with MATLAB Examples Narayan Kovvali Mahesh Banavar Andreas Spanias
YNTHESIS L ECTURES ON ON S ROCESSING SSING S YNTHESIS L ECTURES S IGNAL P ROCE José Moura, Series Editor
An Introduction to Kalma Kal man n Filt Filteri ering ng with MA MATLAB TLAB Examples
Syn Sy nthesis Le Lecctures on Si Sign gnaal Processing Editor José Moura, Carnegie Mello University
Synthesis Lectures in Signal Processing will publish 50- to 100-page books on topics of interest to signal processing engineers and researchers. e Lectures exploit in detail a focused topic. ey can be at different levels of exposition—from a basic introductory tutorial to an advanced monograph—depending on the subject and the goals of the author. Over time, the Lectures will provide a comprehensive treatment of signal processing. Because of its format, the Lectures will also provide current coverage of signal processing, and existing Lectures will be updated by authors when justified. Lectures in Signal Processing are open to all relevant areas in signal processing. ey will cover theory and theoretical methods, algorithms, performance analysis, and applications. Some Lectures will provide a new look at a well established area or problem, while others will venture into a brand new topic in signal processing. By careful reviewing the manuscripts we will strive for quality both in the Lectures’ contents and exposition.
An Introduction to Kalman Filtering with MATLAB MATLAB Examples Narayan Kovvali, Mahesh Banavar, and Andreas Spanias 2013
Sequential Monte Carlo Methods for Nonlinear Discrete-Time Filtering Marcelo G.S. Bruno 2013
Processing of Seismic Reflection Data Using MATLAB™ Wail Wail A. Mousa and Abdullatif A. Al-Shuhail 2011
Fixed-Point Fixed-P oint Signal S ignal Processing Wayne Wayne T. T. Padgett and David V. Anderson 2009
Advanced Radar Detection Schemes Under Mismatched Signal Models Francesco Bandiera, Danilo Orlando, and Giuseppe Ricci 2009
iii
DSP for MATLAB™ and LabVIEW™ IV: LMS Adaptive Filtering Forester W. Isen 2009
DSP for MATLAB™ and LabVIEW™ III: Digital Filter Design Forester W. Isen 2008
DSP for MATLAB™ and LabVIEW™ II: Discrete Frequency Transforms Forester W. Isen 2008
DSP for MATLAB™ and LabVIEW™ I: Fundamentals of Discrete Signal Processing Forester W. Isen 2008
e eory of Linear Prediction P. P. Vaidyanathan 2007
Nonlinear Source Separation Luis B. Almeida 2006
Spectral Analysis Analysis of Signals: e Missing Data Case Yanwei Yanwei Wang, Jian Li, and Petre Stoica 2006
Copyright © 2014 by Morgan & Claypool
All rights reserved. No part of this publication may be reproduced, stored in a retrieval system, or transmitted in any form or by any means—electronic, mechanical, photocopy, recording, recording, or any other except for brief quotations in printed reviews, without the prior permission of the publisher. An Introduction to Kalman Filtering with MATLAB MATLAB Examples Narayan Kovvali, Mahesh Banavar, and Andreas Spanias www.morganclaypool.com
ISBN ISBN:: 9781 978162 6270 7051 5139 3922 ISBN ISBN:: 9781 978162 6270 7051 5140 4088
pape paperba rback ck eboo ebook k
DOI 10.2200/S00534ED1V01Y201309SPR012
A Publication in the Morgan & Claypool Clay pool Publishers series SYNTHESIS LECTURES ON SIGNAL PROCESSING PROCESSING Lecture #12 Series Editor: José Moura, Carnegie Mello University Series ISSN Synthesis Lectures on Signal Processing Print 1932-1236 1932-1236 Electronic Electronic 1932-1694 1932-1694
An Introduction to Kalma Kal man n Filt Filteri ering ng with MA MATLAB TLAB Examples Narayan Kovvali, Mahesh Banavar, and Andreas Spanias SenSIP Center, Arizona State University
SYNTHESIS LECTURES ON SIGNAL PRO PROCESSING CESSING #12
M & C
M o r g a n & cLaypool p u b l i s h e r s
ABSTRACT ABSTRA CT e Kalman filter is the Bayesian optimum solution solution to the problem of sequentially estimating the states of a dynamical system in which the state evolution and measurement processes are both linear and Gaussian. Given the ubiquity of such systems, the Kalman filter finds use in a variety of applications, e.g., target tracking, guidance and navigation, and communications systems. e purpose of this book is to present a brief introduction to Kalman filtering. e theoretical framework of the Kalman filter is first presented, followed by examples showing its use in practical applications. Extensions of the method to nonlinear problems and distributed applications are discussed. A software implementation of the algorithm in the MATLAB programming language is provided, as well as MATLAB code for several example applications discussed in the manuscript.
KEYWORDS dynamical system, parameter estimation, tracking, state space model, sequential Bayesian estimation, linearity, Gaussian noise, Kalman filter
vii
Contents Acknowledgments . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . ix
1
Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1
2
e Est Estima imatio tion n Pr Probl oblem em . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 2.1 2.2 2.3 2.4
3
3.3
4
23
eory . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 23 eory 23 Implem Imp lement entati ation on . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 25 25 3.2.1 Sample MA MATLAB Code Code . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 25 3.2.2 Comp Computatio utational nal Issues Issues . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 25 25 Exampl Exa mples es . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 27 3.3.1 Targe argett Tracki Tracking ng with Radar Radar . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 27 3.3.2 Channe Channell Estimation Estimation in Communica Communications tions Systems Systems . . . . . . . . . . . . . . . . . . 31 3.3.3 Recur Recursive sive Least Least Squares Squares (RLS) (RLS) Adaptive Adaptive Filteri Filtering ng . . . . . . . . . . . . . . . . . 34 34
Extend Ext ended ed and Dec Decent entral ralize ized d Kal Kalman man Filt Filterin ering g . . . . . . . . . . . . . . . . . . . . . . . . . . 43 4.1 4.2
5
Backgroun Backgr ound d. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .. . . . . . . . . . . . . . . . . . . . . . . 5 2.1.1 Examp Example: le: Maximum-L Maximum-Likeli ikelihood hood Estimatio Estimation n in Gaussian Noise Noise . . . . . . . . 6 Linear Lin ear Esti Estimat mation ion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 7 e Bayesian Bayesian Appro Approach ach to Parame Parameter ter Estimati Estimation on . . . . . . . . . . . . . . . . . . . . . . . . . 9 2.3.1 Examp Example: le: Estimati Estimating ng the Bias Bias of a Coin . . . . . . . . . . . . . . . . . . . . . . . . . . 9 Sequential Sequent ial Bayesi Bayesian an Estimatio Estimation n . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 10 2.4.1 Examp Example: le: e 1-D Kalman Kalman Filter Filter . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 18
e Ka Kalm lman an Fil Filte ter r . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 3.1 3.2
5
Extended Extend ed Kalman Kalman Filt Filter er . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 43 4.1.1 Examp Example: le: Predato Predator-Pr r-Prey ey System . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 46 Decentralized Decent ralized Kalman Kalman Filteri Filtering ng . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 49 4.2.1 Examp Example: le: Distribute Distributed d Object Tracki Tracking ng . . . . . . . . . . . . . . . . . . . . . . . . . . 53
Conclusion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
63
Notation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
65
viii
Bibliography . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 67 Authors’ Biographies . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 71
ix
Acknowledgments Acknowledg ments e work in this book was supported in part by the SenSIP Center, Center, Arizona State University. Narayan Kovvali, Mahesh Banavar, and Andreas Spanias September 2013
1
CHAPTER 1
Introduction Statistical estimation is the process of determining the values of certain parameters or signals from empirical (measured or collected) data which is typically noisy and random in nature [ 1, 2 2]. ]. Statistical estimation has application in a multitude of areas. For example, in the area of consumer electronics, estimation techniques are used for mobile wireless communications, intelligent voice and gesture recognition, multimedia enhancement and classification, GPS navigation, and much more. more. In defense and security security rela related ted fields, fields, applicatio applications ns include target tracking, tracking, guidance guidance and navigation systems, and threat detection. Statistical estimation methods also play a vital role in health monitoring and medical diagnosis problems. e flow graph of an estimation problem is shown in Figure 1.1 Figure 1.1.. In the estimation problem, the task is to estimate an unobservable phenomenon of interest (represented by a set of parameters) using observed data. When the parameters vary over time, the estimate may be iteratively updated using continuously obtained data, as shown in Figure 1.2 1.2.. is type of estimation, termed termed sequen sequentia tiall estima estimatio tion, n, involv involves es comput computing ing an initia initiall estima estimate te and then then iterat iterative ively ly updati updating ng the estimate based on the most recent data.
estima imatio tion n pro proble blem, m, the tas taskk is to est estima imate te an uno unobse bservabl rvablee phe phenom nomeno enon n of int intere erest st Figure1.1: In the est (represented by a set of parameters) using observed data.
A large number of statistical estimation algorithms exist, ranging from point estimators such as maximum-likelihood (ML) and maximum a posteriori (MAP) estimators which compute the single best parameter that maximizes the likelihood of the observed data or a posteriori parameter probability, to Bayesian methods which compute the full posterior probability distribution tribution of the parameters. parameters. One class of estimation estimation,, known as linear linear estimation estimation,, computes computes the parameter estimate as a simple linear function of the data. Estimation algorithms are designed to satisfy various optimality criteria, such as consistency, efficiency, unbiasedness, minimum vari-
2
1. INT INTRO RODUC DUCTI TION ON
Figure 1.2: In sequential estimation, time-varying parameters are estimated by computing an initial estimate and then iteratively updating the estimate based on the most recent data.
ance, and minimum mean square error (MMSE) [1 [ 1]. Given a model estimation problem, bounds can be calcul calculate ated d on estima estimatio tion n per perform formanc ance. e. For exampl example, e, the Cramér Cramér-Ra -Raoo low lower er bound bound (CRLB (CRLB)) defines a lower bound on the variance of an estimator [ 1]. e Kalman filter was developed by Rudolph Kalman and Richard Bucy [ 3, 4 4]] in the 60s and is used for the estimation of the parameters (or states) of a linear Gaussian dynamical d ynamical system. Specifically, in a state space setting, the system state must propagate according to a linear evolution model with Gaussian process noise and the data (measurements) must be linearly related to the state with Gaussian noise. Many systems encountered in real-world applications are wellcharacterized by this model. e Kalman filter is a popular choice for estimating the parameters of dynamical systems for several reasons, including: • e Kalman filter is the Bayesian optimum estimator for sequentially estimating the states of a linear Gaussian dynamical system; • e Kalman filtering algorithm has low computational complexity and can be implemented in DSP hardware for realtime applications;
3
• Variati ariations ons and extens extension ionss of the Kalman Kalman filter filter are rea readily dily availa available ble for nonlin nonlinear ear,, distrib distribute uted, d, and non-Gaussian problems, such as the extended Kalman filter, the unscented Kalman filter, the decentralized Kalman filter, and the particle filter [ 5]. e Kalman filter finds application in a variety of problems, for example, target tracking [ 6] and sensor drift correction and inertial navigation [ 7–9]. e rest of this book is organized as follows. Chapter 2 2 introduces introduces the statistical estimation problem and reviews important estimation approaches, such as maximum-likelihood and Bayesian estimators. Chapter 3 3 describes describes the analytical framework of the Kalman filter. Chapter 4 ter 4 discusses discusses extensions of Kalman filtering for f or nonlinear problems and distributed applications. In each chapter, several examples are presented to illustrate the methods and algorithms. Figures are used in the form of block diagrams, system descriptions, and plots throughout the manuscript to illustrate the concepts. MATLAB programs are also provided for most examples. Chapter 5 summarizes the topics covered and provides some references for further reading.
5
CHAPTER 2
e Estimation Problem 2.11 2.
BACK BA CKGR GROU OUND ND
e problem of estimating nonobservable nonobser vable phenomena of interest (termed parameters ) using observed data (e.g., sensor measurements) is a ubiquitous one. Estimation tasks arise in a wide variety of real-world applications, such as medical diagnosis using imaging and bio-signals, weather forecasting based on temperature, pressure, and wind speed readings, and the tracking of target position and velocity using radar or sonar imagery. A general parameter estimation problem [ 1, 10 10,, 11 11]] can be stated as follows. Let x be a D 1 vector that denotes the parameters of interest and y a M 1 vector denoting observed data. e data y carries information about parameters x , quantified using a measurement model that explicitly relates x and y . e measurement model could be deterministic (i.e., it involves no random components), for example, (2.1) y D h .x/; where h W RD 7! RM is a known function (we assume, without loss of generality, that the parameters and data are real-valued). It could also be probabilistic, with a stochastic model used to describe the relationship between x and y : y
D h.x/ C w ;
(2.2)
1 measurement noise vector, or, more generally, where w is a (random) M generally, y
p .y jx/; p.
(2.3)
which directly specifies the conditional probability density function (pdf ) of y given x . e estimation problem is then to determine the parameter vector x using the observed data y , and an estimation algorithm is employed to compute the estimate of x , denoted as Ox. e estimate O .y / depends on the data y . For the models in ( 2.1 2.1)) and (2.2 (2.2), ), this amounts to some sort of an x inversion of the function h: (2.4) O .y / h1.y /: x For (2.3 (2.3), ), a popular estimator for x is the maximum-likelihood [1, 10 10,, 11 11]: ]: maximum-likelihood (ML) estimate [1 xML .y / , argmax p. y x/:
O
x
j
(2.5)
6
2. THE ES ESTI TIMA MATIO TION N PR PROB OBL LEM
e ML estimator is asymptotically optimal or consistent : OxML converges in probability to x as ! 1. An estimate Ox is said to be unbiased if the data set size N ! if
O
Z D O
EΠx.y /
x.y / p. y x/ d y
j
D x;
(2.6)
where E Πdenotes expectation. Figure 2.1 Figure 2.1 shows shows the basic components of the general estimation problem.
Figure 2.1: e general estimation problem is to determine a parameter vector of interest x using observed data y , and an estimation algorithm is employed to compute the estimate Ox.
In many applications, we are interested in estimating parameters that are not static but rather dynamic or varying over time, with the dynamics governed by an evolution law or model. A large number of real-world dynamical systems characterized by stationary and non-stationary random processes fall into this category. For the purpose of estimation, data about the time varying parameters is typically collected at regular intervals of time. In this work we will only consider discrete-time dynamical systems.
2.1.11 2.1.
EXAMPLE: EXAMPL E: MAX MAXIMUMIMUM-LIK LIKEL ELIHOOD IHOOD ESTIM ESTIMA ATION IN GA GAUSSIA USSIAN N NOISE Consider the problem of estimating parameters given data corrupted by additive Gaussian noise. Specifically, we are interested in estimating parameters x from f rom noisy data y of the form y
D h.x/ C w ;
(2.7)
where the function h W RD 7! RM models the relationship between the D 1 parameter vector 1 data vector y , and w is a M 1 Gaussian random measurement noise vector with x and M 2 zero mean and covariance matrix R D y I. e ML estimator OxML .y / for the parameters x is obtained as follows. e likelihood function for the parameters x is given by
j D .2 1 /
p. y x/
2 y
exp M=2
2 1 ky h.x/k 2 y
2
!
;
(2.8)
2.2.. LIN 2.2 LINEAR EAR ES ESTIMA TIMATION TION 7
where kk denotes the 2-norm, and the ML estimator is xML .y /
O
D argmax log p.y jx/; x
"
M log 2 2
D argmax D argmin ky h.x/k ; x
M log y2 2
1 y 2 y2
k h.x/k
2
#
;
(2.9)
2
x
which is the well-known least squares solution. solution. In particular, when the model is linear, i.e., y
D H x C w ;
(2.10)
D matrix, then the ML estimate of the parameters x is given by with H a M xML .y /
O
2.22 2.
2
T
D argmin ky H xk D .H
H/1 HT y :
(2.11)
x
LIN INEA EAR R ES ESTI TIMA MATI TION ON
Before proceeding to general probabilistic estimation, in this section we briefly discuss the frequently encountered setting of linear linear estimation . For our discussion, we pick the specific context of estimating a sampled discrete-time signal from noisy measurements. Let x n denote the signal of interest at time step n 2 ZC and let y n be the corresponding noisy measured signal. e goal is to design a linear estimator for x n of the form M 1
X O D
xn
ai y ni ;
(2.12)
i D0
where a0 ; a1 ; : : : ; aM 1 are filter coefficients, such that the error xn xO n is minimized. In matrix notation, xn
O D
a0
a1 : : :
aM 1
e mean square error is EŒ.xn
2
xO / D D n
EŒx n2
2 664 2 n
ynM C1
2EŒx Ox C EŒ xO 2E x a y C E n
n
M 1
EŒx n2
yn yn1 :: :
" X n
i D0
i
n i
3 77 5
:
# 24 X
2
M 1
i D0
(2.13)
ai y ni
! 35
; (2.14)
8
2. THE ES ESTI TIMA MATIO TION N PR PROB OBL LEM
and differentiating with respect to a i gives @ EŒ.xn @ai
20 X 4@ X
M 1
2
xO / D 2EŒx n
n
yni
C 2E
aj y nj
j D0
M 1
D 2EŒx
n
yni
C2
1 3 A 5 yni
aj E Œynj y ni ;
(2.15)
j D0
for i D 0; 1. Equating this to zero, we see that the desired filter coefficients satisfy 0; : : : ; M M 1
X
aj E E Œynj y ni
j D0
D E Œx
n
yni ;
i
D 0; 1: 0; : : : ; M
(2.16)
When the signals yn and xn are wide-sense stationary (i.e., (i.e., their statistics up to the secondorder are independent of time), with autocorrelation and cross-correlation sequences given respectively by yy
, ri xy , ri
EŒy n yni ; EŒx n yni ;
(2.17a) (2.17b)
Eq. (2.16 (2.16)) becomes M 1
X
yy
D r xy ; i D 0; 1; 0; : : : ; M
aj r i j
j D0
i
(2.18)
which are the Wiener-Hopf equations [12 [ 12]. ]. Note that the autocorrelation and cross-correlation yy yy xy yx sequences obey symmetry properties: r i D r i and r i D r i . In matrix notation, (2.18 ( 2.18)) can be written as yy yy yy xy
or
2 66 4
r0 yy r1 :: :
yy
rM 1
r1 yy r0 :: :
yy
rM 2
: : : rM 1 yy : : : rM 2 :: :: : : yy ::: r0
32 77 66 54
a0 a1 :: :
aM 1
R yy a
D r xy :
e Wiener filter coefficients coefficients are given by
D .R yy /
3 2 77 D 66 5 4
r0 xy r1 :: :
xy
rM 1
1 xy
a
;
(2.19)
(2.20)
r :
3 77 5
(2.21)
e matrix R yy is symmetric and positive-semidefinite, and a unique solution almost always exists for (2.21 (2.21). ). Since the signals involved are stationary, the filter is time-invariant. e linear estimate for x n is then computed using (2.13 ( 2.13)) as xn
O D a
T
yn ;
(2.22)
2.3.. THE BA 2.3 BAYES YESIAN IAN APPR APPROA OACH CH TO PARA ARAMET METER ER ES ESTIMA TIMATION TION 9
where a D Œa 0 a1 : : : aM 1 T obta obtain ined ed in (2.21 2.21), ), and and yn D Œy n yn1 : : : ynM C1 T is the the M 1 measurement vector at time step n .
2.33 2.
THE BA BAYE YESI SIAN AN AP APPR PRO OACH TO PAR ARAM AMET ETER ER ESTIMATION
In the Bayesian approach [1 [ 1, 10 10,, 11 11,, 13 13]] to estimation, the parameters of interest are probabilistically estimated by combining two pieces of information: (a) belief about the parameters based on the likelihood of the observed data as stipulated by a probabilistic measurement model, and (b) a priori knowledge about the parameters quantified probabilistically by a prior pdf. Specifically, in Bayesian inference the posterior pdf p. p .xjy / over the parameters x given the data y is computed by combining the likelihood p. [ 11]: ]: p .y jx/ and prior p. p .x/ using Bayes’ theorem [11
jx/ p.x/ D j D p.yp. y/
p. x y /
p. y x/ p. x/ p. y x/ p. x/ d x
R
j
j
/ p. p .y jx/ p. x/:
(2.23)
Eq. (2.23 (2.23)) prescribes how a priori belief p. x/ about the parameter x is updated to the a posteriori belief p. is typica t ypically lly defined as the mean p .xjy / in light of the observed data y . e Bayes estimate is of the posterior pdf:
Z j D
x .y / , E Œx y
O
B
x p. x y / d x:
j
(2.24)
e Bayes estimator in ( 2.24 2.24)) is optimum in the sense that it minimizes the mean square error [ 1] (MSE) Bayes risk [1 E Œ.x
B
Ox
T
.y // .x
B
Ox
.y //
“ D
.x
OxB.y //
T
.x
OxB.y //p.x; y / d x d y :
(2.25)
In other words xB .y /
O
T
D argmin E Œ.x Ox.y // x O .y /
.x
Ox.y //:
(2.26)
Additionally, Additionally, the Bayes estimator is asymptotically unbiased. As seen in (2.23 2.23)) and (2.24 (2.24), ), Bayesian estimation utilizes Bayes’ theorem to compute the desired parameter estimate. Bayes’ theorem in its current form was developed as a generalization of the work of Bayes [14 [ 14]. ]. With the advent of powerful computers, Bayesian estimation is applied today in many real-world problems with complex statistical models using Monte Carlo techniques [15 [15,, 16 16]. ].
2.3.11 EX 2.3. EXAM AMPL PLE: E: ES ESTIM TIMA ATIN TING G THE BI BIAS AS OF A CO COIN IN In a coin coin flipp flippin ingg expe experi rime ment nt,, a coin coin is toss tossed ed N times times and the outco outcome me ‘heads’ ‘heads’ is observe observed d r times. Based on this data, we wish to estimate the probability of heads pH for this coin. Following the
10
2. THE EST ESTIMA IMATION TION PRO PROBL BLEM EM
Baye Bayesi sian an appr approa oach ch,, we start start by defin definin ingg a prio priorr pdf pdf over over pH , take taken n here here to be the the stan standa dard rd unif unifor orm m distribution: 1; for 0 pH 1; (2.27) p.pH / D 0; otherwise.
Given pH , the probability of observing heads r times in N coin tosses is Binomial and given by p.r N; pH /
j
D
!
N r pH .1 r
p
H /
N r
;
for r D 0; 0; : : : ; N :
(2.28)
Combining the prior in (2.27 ( 2.27)) with the likelihood in (2.28 ( 2.28)) using Bayes’ theorem, we obtain the posterior pdf of p pH as the Beta distribution:
(
1 p r .1 B.r C1;N r C1/ H
N r ; H /
for 0 pH 1; otherwise, 0; (2.29) where B. B .; / denotes the Beta function. e Bayes estimate of pH is then the mean of the Beta posterior pdf, Beta .pH I r C 1; N [ 2] r C 1/, and is given by [2 p.pH r ; N /
p .r jN; p / p.r
j
H /p.pH /
D
1
B pH
Z D
O D E Œp jr ; N H
p
1 I C 1; N r C 1/ dp D N r C C C 2 :
pH Beta.pH r
0
H
(2.30)
(2.31)
It should be mentioned here that the ML estimate of pH is ML pH
O D argmax p.r jN; p / D argmax H
pH
pH
!
N r pH .1 r
N r H /
p
D N r :
Figure 2.2f Figure 2.2f shows shows plots of the Beta posterior pdf for various r and N . Observe that the variance of the posterior pdfs decreases d ecreases with increasing data. e ML estimation method simply yields a point estimate, unlike the Bayesian estimation approach approach which provides the full posterior pdf that can be used to assess confidence in the estimate.
2.44 2.
SEQU SE QUEN ENTIA TIAL L BA BAYE YESI SIAN AN ES ESTIM TIMA ATIO TION N
In dynamical systems with parameters evolving over time, a convenient framework for representation is provided by state of the form state space models of xn yn
p. xn xn1 /; p. yn xn /;
j j
(2.32a) (2.32b)
where xn is the D 1 state vector (parameters) at time step n 2 ZC and y n is the corresponding 1 measurement vector (data). e state vector xn might, for example, represent the position M and velocity of a moving target at time step n that need to be estimated, with the measurement
2.4.. SEQUE 2.4 SEQUENTIA NTIAL L BA BAYES YESIAN IAN EST ESTIMA IMATIO TION N
11
10 9 8 7 f d 6 p r o i r 5 e t s o P 4
3 2 1 0 0
0.2
0.4
0.6
0.8
1
Probability of heads B ML (a) r D 7, N D 10, pOH D 2=3 , pOH D 7=10
probability of heads pH for a coin, when the outcome ‘heads’ Figure 2.2a: Bayesian estimation of the probability is observed to occur r times in N coin tosses. tosses. e plots show the posterior posterior pdf p .pH jr;N/, which, r C 1). e Bayes and ML estimates under a uniform prior, is the Beta distribution Beta( r C 1; N C 2 and r r=N pH are r C 1=N C =N , respectively. of p
12
2. THE EST ESTIMA IMATION TION PRO PROBL BLEM EM
10 9 8 7 f d 6 p r o i r 5 e t s o P 4
3 2 1 0 0
0.2
0.4
0.6
0.8
1
Probability of heads B ML (b) r D 21, N D 30, pOH D 11=16, pOH D 7=10
probability of heads pH for a coin, when the outcome ‘heads’ Figure 2.2b 2.2b:: Bayesian estimation of the probability is observed to occur r times in N coin tosses. tosses. e plots show the posterior posterior pdf p .pH jr;N/, which, under a uniform prior, is the Beta distribution Beta( r C 1; N r C 1). e Bayes and ML estimates pH are r C 1=N C =N , respectively. of p C 2 and r r=N
2.4.. SEQUE 2.4 SEQUENTIA NTIAL L BA BAYES YESIAN IAN EST ESTIMA IMATIO TION N
13
10 9 8 7 f d 6 p r o i r 5 e t s o P 4
3 2 1 0 0
0.2
0.4
0.6
0.8
1
Probability of heads B ML (c) r D 63, N D 90, pOH D 16=23, pOH D 7=10
Figure 2.2c 2.2c:: Bayesian estimation of the probability of heads pH for a coin, when the outcome ‘heads’ is observed to occur r times in N coin tosses. tosses. e plots show the posterior posterior pdf p .pH jr;N/, which, r C 1). e Bayes and ML estimates under a uniform prior, is the Beta distribution Beta( r C 1; N C 2 and r r=N pH are r C 1=N C =N , respectively. of p
14
2. THE EST ESTIMA IMATION TION PRO PROBL BLEM EM
25
20
f d15 p r o i r e t s o P10
5
0 0
0.2
0.4
0.6
0.8
1
Probability of heads B ML (d) r D 5, N D 5, pOH D 6=7, pOH D1
the probability of heads pH for a coin, when the outcome ‘heads’ Figure 2.2d 2.2d:: Bayesian estimation of the is observed to occur r times in N coin tosses. tosses. e plots show the posterior posterior pdf p .pH jr;N/, which, under a uniform prior, is the Beta distribution Beta( r C 1; N r C 1). e Bayes and ML estimates pH are r C 1=N C =N , respectively. of p C 2 and r r=N
2.4.. SEQUE 2.4 SEQUENTIA NTIAL L BA BAYES YESIAN IAN EST ESTIMA IMATIO TION N
15
25
20
f d15 p r o i r e t s o P10
5
0 0
0.2
0.4
0.6
0.8
1
Probability of heads B ML (e) r D 10, N D 10, pOH D 11=12, pOH D1
Figure 2.2e 2.2e:: Bayesian estimation of the probability of heads pH for a coin, when the outcome ‘heads’ is observed to occur r times in N coin tosses. tosses. e plots show the posterior posterior pdf p .pH jr;N/, which, r C 1). e Bayes and ML estimates under a uniform prior, is the Beta distribution Beta( r C 1; N C 2 and r r=N pH are r C 1=N C =N , respectively. of p
16
2. THE EST ESTIMA IMATION TION PRO PROBL BLEM EM
25
20
f d15 p r o i r e t s o P10
5
0 0
0.2
0.4
0.6
0.8
1
Probability of heads B ML (f ) r D 20, N D 20, pOH D 21=22, pOH D1
Figure 2.2f 2.2f:: Bayesian estimation of the probability of heads pH for a coin, when the outcome ‘heads’ is observed to occur r times in N coin tosses. tosses. e plots show the posterior posterior pdf p .pH jr;N/, which, under a uniform prior, is the Beta distribution Beta( r C 1; N r C 1). e Bayes and ML estimates pH are r C 1=N C =N , respectively. of p C 2 and r r=N
2.4.. SEQUE 2.4 SEQUENTIA NTIAL L BA BAYES YESIAN IAN EST ESTIMA IMATIO TION N
17
vector yn com compri prised sed of range range and rangerange-rat ratee informa informatio tion n collec collected ted at time time step step n by a rada radarr syste system. m. Eq. (2.32a (2.32a)) specifies a first-order Markov [2 [ 2] state evolution law for the time-varying state, in which the state x n at any time step n is independent of the states x n2 ; xn3 ; : : : ; x0 at the past time steps n 2; n 3 ; : : : ; 0 given the state xn1 at the immediate previous time step n 1. Eq. (2.32b (2.32b)) describes the relationship between the time-varying state and the measurements, in which the measurement yn at any any time time step step n is condit condition ionally ally indepe independe ndent nt of all other other states states given given the state x n at that time step n . Note that both state and measurement models are probabilistic. e initial state distribution is given by the pdf p. x0 /. Figure igure 2.3 shows shows graphi graphical cally ly the state state space space model for a dynamical system.
Figure 2.3: State space model for a dynamical system. e time-varying state evolves according to a first-order Markov model, in which the state xn at any time step n is independent of the states xn2 ; xn3 ; : : : ; x0 at the past time steps n 2; n 3 ; : : : ; 0 given the state x n1 at the immediate previous time step n 1. e measurement y n at any time step n is conditionally independent of all other states given the state xn at that time step n. e initial state distribution is given by the pdf p. x0 /.
In the state space setting, two key tasks of interest are: (a) filtering , and (b) prediction. e filtering problem is to estimate the state x n at time step n given the set Y n , fy1 ; y2 ; : : : ; yn g of measurements up to time step n . e prediction problem is to estimate the state(s) x nCl at time step n C l , l > 0, given the set Y n of measurements up to time step n . e Bayesian optimum solution to the filtering problem sequentially sequ entially or recursively computes the posterior pdf p. p. xn jY n / over the state xn given the measurement set Y n in two steps. In the first step, known as the prediction step, the Markov state evolution model p. xn jxn1 / is used with the posterior pdf p. xn1 jY n1 / at the the previ previou ouss time time step step n 1 to obta obtain in the the pdf pdf p. xn jY n1 / over the state xn given the measurement set Y n1 . In the second step, known as the update step,
18
2. THE EST ESTIMA IMATION TION PRO PROBL BLEM EM
the posterior pdf p. p. xn jY n / is computed by combining the measurement likelihood p. yn jxn / and prediction p. xn jY n1 / using Bayes’ theorem. Mathematically, Mathematically,
Z D
Predict step:
p. xn Y n1 /
Update step:
p. xn Y n /
j
p. xn xn1 / p. xn1 Y n1 / d xn1 ;
j
j
j / p. p .y jx / p. x jY /; n
n
n
(2.33a)
n 1
(2.33b)
for n D 1; 1; 2 ; : : : , with p. p .x0 jY 0 / , p. x0 / used to initialize the iteration. Observe, by comparing with (2.23 2.23), ), that at each time step here the prediction p. xn jY n1 / plays the role of the prior pdf. e Bayesian filtering iteration of Eq. (2.33 ( 2.33)) is a recursive computation of the posterior pdf p. xn jY n / at each time step n : p. x0 Y 0 /
j
predict
! p.x jY / update ! p.x jY / predict ! : : : : : : update ! p.x jY / predict ! : : : 1
0
1
1
n
n
(2.34)
for n D 1; 1; 2 ; : : : . e estimate for x n is obtained using the mean of the posterior pdf: xn .Y n /
Z D
D E Œx jY
O
n
n
xn p. xn Y n / d xn :
j
(2.35)
e sequential Bayesian estimation procedure is shown graphically in Figure Figure 2.4 2.4.. e prediction problem is solved by recursively computing the posterior pdfs p. p .xnCl jY n / over the states xnCl given the measurement set Y n as
Z D
p. xnCl Y n /
j
p. xnCl xnCl 1 / p. xnCl 1 Y n / d xnCl 1 ;
j
j
(2.36)
for l D 1; 1; 2 ; : : : , and the estimates for x nCl are again the means of the posterior pdfs.
2.4.11 EX 2.4. EXAM AMPL PLE: E: THE 11-D D KA KALM LMAN AN FI FIL LTE TER R Consider a one-dimensional (1-D) linear Gaussian state space model xn yn
D D
F n xn1 vn ; H n xn wn ;
C C
(2.37a) (2.37b)
where xn is a scalar state at time step n, yn is the corresponding scalar measurement, F n is a linear state-transition parameter, parameter, v n is Gaussian random state noise with zero mean and variance Q n , parameter, and wn is Gaussian random measurement noise with zero H n is a linear measurement parameter, mean and variance Rn . e initial state distribution p.x 0 / is Gaussian. e task is to sequentially estimate the state x n using the set of measurements Y n D fy1 ; y2 ; : : : ; yn g. We We follow the sequential Bayesian estimation procedure described earlier. Suppose that at time step n 1 the posterior pdf p.x p.x n1 jY n1 / is Gaussian: p.x n1 Y n1 /
j
N .x I m j ; P j /; n 1
n 1n 1
n 1n 1
(2.38)
2.4.. SEQUE 2.4 SEQUENTIA NTIAL L BA BAYES YESIAN IAN EST ESTIMA IMATIO TION N
19
p. xn jY n / over the Figure 2.4 Figure 2.4:: Sequential Bayesian estimation recursively computes the posterior pdf p. state x n given the measurement set Y n D fy1 ; y2 ; : : : ; yn g. e estimate for x n is obtained using the mean of the posterior pdf: Oxn .Y n / D E Œxn jY n .
where the notation N . I m ; P / is used to denote a Gaussian pdf with mean m and variance P . us in (2.38 2.38)) mn1jn1 and P n1jn1 denote, respectively, the Gaussian posterior mean and variance at time step n 1. e subscript notation ‘ n 1jn 1’ is used to indicate association with the (Gaussian) pdf of the state x n1 at time step n 1 computed using the measurements up to time step n 1. From the model (2.37 ( 2.37), ), the conditional pdfs p.x p .xn jxn1 / and p.y p .yn jxn / of the state and measurement, respectively, are given by the Gaussian pdfs p.x n xn1 / p.y n xn /
j
j
N .xn F n xn1 ; Qn /; N .yn H n xn ; Rn /:
I I
(2.39a) (2.39b)
e prediction step ( 2.33a 2.33a)) therefore becomes
Z D j Z D p
p.x n Y n1 /
j
p.x n xn1 /p.xn1 Y n1 / dx n1 1
2Qn
j
e
.xn F n xn1 /2 2Qn
1
p
2P n1jn1
e
.xn1 mn1jn1 /2 2P n1jn1
dx n1 ; (2.40)
20
2. THE EST ESTIMA IMATION TION PRO PROBL BLEM EM
which is a convolution of Gaussian functions and results in a Gaussian predicted state distribution p.xn Y n1 /
j
N .x I m j ; P j /; n
nn 1
nn 1
(2.41)
with mean and variance given by mnjn1 P njn1
F n mn1jn1 ; F n2 P n1jn1
D D
(2.42a) (2.42b)
CQ : n
e update step (2.33b ( 2.33b)) is now a product of Gaussian functions p.x n Y n /
j
/ p.y jx /p.x jY / 1 e D p 2R n
n
n
n 1
1
.yn H n xn /2 2Rn
p
2P njn1
n
resulting in a Gaussian posterior pdf at time step n
e
.xn mnjn1 /2 2P njn1
j N .x I m j ; P j /;
p.xn Y n /
n
nn
;
nn
(2.43)
(2.44)
with mean and variance given by mnjn P njn
mnjn1 P njn1
D D
where
C K .y H m j /; K H P j ; n
n
n
n
n
nn 1
D H P P j H C R
K n
nn 1
2 n
n
:
(2.45a) (2.45b)
(2.46)
nn 1
n
njn1
Since the initial state distribution p.x p .x0 / is Gaussian, under the stated conditions, all pdfs in the Bayesian recursion steps of Eq. (2.33 ( 2.33)) assume a Gaussian form. e Bayesian filtering iteration of Eq. (2.33 (2.33)) then simplifies to a recursive computation of the mean m njn and variance variance P njn of the Gaussian posterior pdf p.x p.x n jY n / at each time step n :
fm j ; P j g predict ! fm j ; P j g update ! fm j ; P j g predict ! : : : : : : update ! fm j ; P j g predict ! : : : 00
00
10
10
11
11
nn
nn
(2.47)
via the set of equations mnjn1 P njn1 S n K n mnjn P njn
D D D D D D
F n mn1jn1 ; 2 F n P n1jn1 Qn ; 2 H n P njn1 Rn ; P njn1 H n =S n ; mnjn1 K n .yn H n mnjn1 /; P njn1 K n H n P njn1 ;
C C
C
(2.48a) (2.48b) (2.48c) (2.48d) (2.48e) (2.48f)
2.4.. SEQUE 2.4 SEQUENTIA NTIAL L BA BAYES YESIAN IAN EST ESTIMA IMATIO TION N
for n D 1; [ 6, 12 12]. ]. 1; 2 ; : : : , which is the 1-D Kalman filter [6 In what follows, the Kalman filter will be described formally as the estimator of the parameters of a dynamical system whose evolution and measurement can be modeled using a linear Gaussian Gaussian state space representatio representation. n. Examples Examples are provided, provided, as well as extensions extensions to nonlinear nonlinear problems and distributed systems.
21
23
CHAPTER 3
e Kalman F ilter 3.1
THEORY
e Kalman filter [3 [ 3, 4 4]] is the Bayesian solution to the problem of sequentially estimating the states of a dynamical system in which the state evolution and measurement processes are both linear and Gaussian. Consider a state space model of the form xn yn
D D
Fn xn1 vn ; Hn xn wn ;
C C
(3.1a) (3.1b)
where Fn is the D D state-transition matrix, vn is a D 1 Gaussian random state noise vector D measurement matrix, and w n is a with zero mean and covariance matrix Q n , H n is the M 1 Gaussian random measurement noise vector with zero mean and covariance matrix R n. M e state space model ( 3.1 3.1)) describes a dynamical system in which the state evolution and measurement processes are both linear and Gaussian. e conditional pdfs p. xn jxn1 / and p. yn jxn / of the state and measurement vectors, respectively, are then given by the Gaussian pdfs p. xn xn1 / p. yn xn /
j
j
N .xn Fn xn1 ; Qn /; N .yn Hn xn ; Rn /;
I I
(3.2a) (3.2b)
where the notation N . I m; P / is used to denote a Gaussian pdf with mean m and covariance P . Suppose that the initial state distribution p. x0 / is also Gaussian. Following the sequential Bayesi Bayesian an estima estimatio tion n pro proced cedure ure descri described bed in Sectio Section n 2.4 2.4,, under under the stated stated condit condition ions, s, the poster posterior ior pdf p. p. xn jY n / at time step n can be shown to be Gaussian:
j N .x I m j ; P j /;
p. xn Y n /
n
nn
nn
(3.3)
where mnjn and P njn denote the Gaussian posterior mean and covariance at time step n. is is a direct consequence of all pdfs in the Bayesian recursion steps of Eq. ( 2.33 2.33)) assuming a Gaussian form. e subscript notation ‘ njn’ is used to indicate association with the (Gaussian) pdf of the state vector xn at time step n computed using the measurements up to time step n. e Bayesian filtering iteration of Eq. (2.33 ( 2.33)) then reduces to a recursive computation of the mean m njn and covariance P njn of the Gaussian posterior pdf p. p .xn jY n / at each time step n :
fm j ; P j g predict ! fm j ; P j g update ! fm j ; P j g predict ! : : : : : : update ! fm j ; P j g predict ! : : : (3.4) 00
00
10
10
11
11
nn
nn
24 3. THE KA KALM LMAN AN FIL FILTE TER R
e Kalman filter equations are given by [ 6, 12 12]]
D D D D D D
mnjn1 P njn1 Sn Kn mnjn P njn
Fn mn1jn1 ; T Fn P n1jn1 Fn Qn ; T Hn P njn1 Hn Rn ; T 1 P njn1 Hn Sn ; mnjn1 Kn .yn Hn mnjn1 /; P njn1 Kn Hn P njn1 ;
C C
C
(3.5a) (3.5b) (3.5c) (3.5d) (3.5e) (3.5f)
for n D 1; ( 3.5a)–( )–(3.5b 3.5b)) represent the prediction step and Eqs. (3.5c ( 3.5c)–( )–(3.5f 3.5f ) 1; 2 ; : : : . Here, Eqs. (3.5a comprise the update step. Note that these equations are the multivariate versions of the 1-D Kalman Kalman filter filter equ equati ations ons (2.48 2.48)) in Subse Subsect ctio ion n 2.4.1 2.4.1.. e Kalman Kalman filteri filtering ng algori algorithm thm is summar summarized ized below. Algorithm 1 Kalman filtering 3.1), ), and a set of measurements fy1 ; y2 ; : : : ; yN g. Input : Linear Gaussian state space model ( 3.1 1;; 2 ; : : : ; N . Output : Estimates of the states x 1 ; x2 ; : : : ; xN at time steps n D 1 Initialize the mean and covariance of the Gaussian state distribution N .x0 I m0j0 ; P 0j0 / at time step n D 0 . For time steps n D 1 1;; 2 ; : : : ; N , perform the following: 1. Carry out the predict prediction ion step mnjn1 P njn1
D D
Fn mn1jn1 ; Fn P n1jn1 FnT
CQ : n
2. Comp Compute ute the estimat estimatee Oxn D m njn of the state x n using the update step Sn Kn mnjn P njn
D D D D
Hn P njn1 HT Rn ; n T 1 P njn1 Hn Sn ; mnjn1 Kn .yn Hn mnjn1 /; P njn1 Kn Hn P njn1 :
C
C
3.2. IMPL IMPLEMEN EMENT TATION 25
3.2
IMPL IMP LEM EMEN ENT TATIO TION N
3.2.11 SA 3.2. SAMP MPL LE MA MATL TLAB AB CO CODE DE We We give a sample MATLAB MATLAB implementation of the Kalman filter. filter. e MATLAB MATLAB function kalman_filter.m shown in Listing 3.1 Listing 3.1 performs performs one iteration of the Kalman filter prediction and update steps (Eq. (3.5 ( 3.5)). )). is function will be used in all of the Kalman filtering examples
Figure 3.1: Generic scenario of a static ground-based radar system tracking a moving ground target. Information about the target’s dynamics is combined with delay-Doppler (range and range-rate) measurements obtained from the radar to compute an estimate of the target’s position and velocity.
given in Section 3.3 3.3.. Note the use of the MATLAB ‘/’ operator in implementing the update step (3.5d (3.5d), ), that avoids explicit computation of the inverse of the matrix S n .
3.2.2 COM 3.2.2 COMPUT PUTA ATION TIONAL AL ISSUE ISSUES S e matrix-vector operations in the Kalman filter prediction and update steps in general involve complexity. e computational complexity, complexity, storage requirements, O.D 2 /, O.MD/, and O .M 3 / complexity. and numeri numerical cal stabil stability ity,, can be optimi optimized zed by exploi exploitin tingg structu structure re.. For exampl example, e, the covari covarianc ancee matrices are symmetric and positive-semidefinite and the Cholesky factorization [ 17 17]] can be utilized D L LT , where L is a lower trianguto represent and maintain them in the square root form P D lar matrix [6 [ 6, 12 12]. ]. In addition, depending on the application, it may be possible to improve the
26 3. THE KA KALM LMAN AN FIL FILTE TER R
1
function [m_n,P_n] = kalman_filter kalman_filter (m_n1,P_n1,y_n,F_n,Q_ (m_n1,P_n1,y_n,F_n,Q_n,H_n,R_n) n,H_n,R_n)
2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18
% % % % % % % % % % % % % % % %
Kalman filter Kalman filter pre predic dictio tion n and update update ste steps ps --- propagat propagates es Gau Gaussi ssian an poster pos terior ior state state distribu distributio tion n fro from m tim time e ste step p n-1 to tim time e ste step p n (for (f or detai details ls, , se see e Se Sect ctio ion n 3. 3.1 1 of the text) text) Inputs: Inputs: m_n1 m_ n1 - (D (Dx1 x1 ve vect ctor or) ) Ga Gaus ussi sian an po post ster erio ior r me mean an at ti time me st step ep nn-1 1 P_n1 P_n 1 - (Dx (DxD D mat matrix rix) ) Gau Gaussi ssian an posterio posterior r cov covari arianc ance e at time step n-1 y_n y_ n - (M (Mx1 x1 vecto vector) r) measu measure reme ment nts s at time time st step ep n F_n - (Dx (DxD D mat matrix rix) ) sta statete-tra transi nsitio tion n mat matrix rix at tim time e ste step p n Q_n Q_ n - (D (DxD xD ma matr trix ix) ) Ga Gaus ussi sian an st stat ate e no nois ise e co cova vari rian ance ce at time step step n H_n H_ n - (M (MxD xD matrix matrix) ) me meas asur urem emen ent t ma matr trix ix at ti time me step step n R_n - (Mx (MxM M mat matrix rix) ) Gau Gaussi ssian an measurem measurement ent noise noise cov covari arianc ance e at time step n Outputs: Outputs: m_n m_ n - (D (Dx1 x1 vector vector) ) Gaus Gaussi sian an poste posteri rior or mean at ti time me step step n P_n - (Dx (DxD D mat matrix rix) ) Gau Gaussi ssian an posteri posterior or covaria covariance nce at time step n
19 20 21 22
% Pred Predict ict m_nn1 m_n n1 = F_n*m_ F_n*m_n1; n1; P_nn1 P_n n1 = F_n F_n*P_ *P_n1* n1*F_n F_n' ' + Q_n Q_n; ;
23 24 25 26 27 28
% Upd Update ate S_n = H_n*P_n H_n*P_nn1* n1*H_n H_n' ' + R_n R_n; ; K_n = P_nn1*H P_nn1*H_n'/S _n'/S_n; _n; m_n = m_nn1 + K_n*(y_nK_n*(y_n-H_n*m H_n*m_nn1) _nn1); ; P_n = P_nn1 P_nn1 - K_n K_n*H_ *H_n*P n*P_nn _nn1; 1;
Listing 3.1: kalman_filter.m kalman_filter.m
3.3.. EX 3.3 EXAMP AMPLE LES S
27
efficiency efficiency further by leveraging leveraging sparsity and the availabili availability ty of any fast transforms transforms (eg. FFT). FF T). It should be noted that the cost remains the same for each Kalman iteration.
3.33 3.
EXAMPL PLE ES
e Kalman filter finds use in a wide variety of applications, e.g., target tracking [ 6], guid guidan ance ce and and navigatio navigation n [7–9], biomed biomedica icall signal signal pro proces cessin singg [ 18 18], ], motio motion n estima estimatio tion n [ 19 19], ], and communicat communications ions systems [20 [20]. ]. We now present several examples showing Kalman filtering in action.
3.3.1 TAR 3.3.1 ARGET GET TRA TRACK CKING ING WITH RA RAD DAR We We consider the problem of radar-based tracking of a moving target. Figure 3.1 3.1 depicts depicts a generic scenario in which a static ground-based radar system tracks a moving ground target. In our example, we let the target move in the x-direction only, and denote with xn the target’s x-position at time step n and with xP n its x-velocity at time step n. Assuming that the target moves with approximately constant velocity, the linear dynamical model of the target’s motion can be written as xn xn
D x C t Px C v ; P D xP C v ; where t is the time interval between time steps n 1 and n, and v dom perturbations with zero mean and covariances E Œv D and E Œv D t [6]. In matrix notation, 2 n;2
n 1
n 1
n 1
n;2
D xn xn
P
1 0
C
t 1
(3.6a) (3.6b)
n;1 and vn;2 are Gaussian ran3 2 2 .t/ 2 .t/ , , E Œv v n;1 n;2 x x 3 2
2 n;1
2 x
n;1
xn1 xn1
vn;1 vn;2
P
D
:
(3.7)
us, the state vector xn D Œx n xP n T is two-dimensional ( D D 2 ), the state-transition matrix Fn is 2 2 and given by
D
Fn
1 0
t 1
;
(3.8)
and the state noise vector vn D Œv n;1 vn;2 T is 2 1 and Gaussian with zero mean and covariance matrix 2 x
D
Qn
"
.t/3 3 .t/2 2
.t/2 2
t
#
:
(3.9)
e radar system utilizes time-delay and Doppler shift of transmitted and received signals to obtain noisy measurement measurementss of the target’s target’s range r n (position x n ) and range-rate Prn (velocity xP n ). e measurement process can be written as rn rn
D x Cw P D xP C w n
n;1 ;
n
n;2 ;
(3.10a) (3.10b)
28 3. THE KA KALM LMAN AN FIL FILTE TER R
where w n;1 and w n;2 are i.i.d. Gaussian with zero mean and variance y2 . In matrix notation,
D C rn rn
xn xn
P
wn;1 wn;2
P
:
(3.11)
D 2), the measurement ma us, the measurement vector vector yn D Œr n rPn T is two-dimensional ( M D trix Hn D I is the 2 2 identity matrix, and the measurement noise vector wn D Œw n;1 wn;2 T is 2 1 and Gaussian with zero mean and covariance matrix R n D y2 I . e state evolution and measurement models are both linear and Gaussian, and the Kalman filter can be used to estimate the moving target’s position and velocity. e MATLAB program Listing 3.2 performs performs N D D 100 time steps of target tracking using target_tracking.m shown in Listing 3.2 the Kalman Kalman filter filter.. e state state space space model model par parame ameter terss wer weree t D 0:5 , x D 0:1 , y D 0:1 , the the init initia iall T target position and velocity were set as x 0 D Œ1 1 , and the Kalman filter was initialized with Gaussian mean m0j0 D x 0 and covariance P 0j0 D I . e MATLAB function kalman_filter.m shown in Listing 3.1 Listing 3.1 is is utilized here to carry out the Kalman filter prediction and update steps (Eq. (3.5 (3.5)). )). Figure 3.2 Figure 3.2 shows shows a plot of the actual and estimated target position and velocity. velocity. It can be seen that the Kalman filter is able to track the moving target accurately. Target T arget Tracking in Higher Dimensions
We We now consider the radar-based tracking of a target moving in both x- and y- directions, and denote with .xn ; yn / the target’s position at time step n and with .xP n ; Pyn / its velocity at time step n. Assuming that the target moves with approximately constant velocity, the linear dynamical model of the target’s motion can be written in matrix notation as xn yn xn yn
2 64 P
P
3 2 75 D 64
1 0 0 0
0 1 0 0
t t 0 1 0
0 t 0 1
xn1 yn1 xn1 yn1
32 75 64 P
P
3 2 75 C 64
vn;1 vn;2 vn;3 vn;4
3 75
;
(3.12)
where v n;1 through v n;4 are zero mean correlated Gaussian random perturbations as before. In this case, the state vector xn D Œx n yn xP n yPn T is four-dimensional ( D D 4 ), the state-transition matrix F n is 4 4 and given by
2 6 D 4
Fn
1 0 0 0
0 1 0 0
t t 0 1 0
0 t 0 1
3 75
;
(3.13)
3.3.. EX 3.3 EXAMP AMPLE LES S 1
% Rad Radar ar based Target Target Tracking Tracking using using the Kalman Kalman Filter Filter
2 3 4 5 6 7 8 9 10
% State State space space mode model l delt = 0.5; % Time step interval F_n = [1 delt; 0 1]; % State-transition matrix sigma_x sigma _x = 0.1; Q_n = sigm sigma_x^2 a_x^2*[del *[delt^3/ t^3/3 3 delt^2/2;... delt^2/2;... delt^2/2 delt]; % State noise covariance matrix H_n = eye(2); % Measurement matrix si sigma_y = 0.1; % Measurement noise standard deviation R_n R_ n = si sigm gma_ a_y^ y^2* 2*ey eye( e(2) 2); ; % Mea Measu sure reme ment nt no nois ise e cov covar aria ianc nce e mat matri rix x
11 12 13 14 15
% Initiali Initializatio zation n x(1:2,1) = [1; 1]; m(1: m( 1:2 2,1 ,1) ) = x( x(1: 1:2, 2,1 1); P(1: P( 1:2, 2,1: 1:2, 2,1) 1) = eye eye(2 (2); );
% Initial target x-position and x-velocity % Ga Gau uss ssia ian n po post ster eri ior me mean an at tim ime e st step ep 1 % Gau Gauss ssia ian n pos poste teri rior or co cova vari rian ance ce at ti time me st step ep 1
16 17 18
% Track tar target get using using Kalman Kalman filter filter for n = 2 : 100,
% Time steps
19 20 21 22
% Stat State e prop propagat agation ion v_n = mvnrnd([0 0],Q_n)'; x(1: x( 1:2, 2,n) n) = F_ F_n* n*x( x(1: 1:2, 2,nn-1) 1) + v_ v_n; n;
% State noise vector % Ma Mark rkov ov li line near ar Ga Gaus ussi sian an ev evol olut utio ion n
% Gene Generate rate measu measuremen rements ts w_n = sigma_y*randn(2,1); y_n = H_n*x(1:2,n) + w_n;
% Measurement noise vector % Linear Gaussian measurements
23 24 25 26 27 28 29 30 31
% Compu Compute te Gaus Gaussia sian n poste posterio rior r mean mean and cov covari arianc ance e at time time step step n [m(1:2,n),P(1:2,1:2,n)] [m(1:2,n),P(1: 2,1:2,n)] = kalman_filter (m(1:2,n-1),... P(1:2,1:2,n-1),y_n,F_n,Q_n,H_n,R_n); end
32 33 34 35 36 37
% Plo Plot t act actual ual and est estima imated ted target target pos positi ition on and velocit velocity y figure, plot(x(1,:),x(2,:),'b'); plot(x(1,:),x(2,:),'b'); hold hold on, plot(m(1,:),m(2,:),'r--'); plot(m(1,:),m(2,:),'r--'); xlabel('target x-position'); ylabel('target ylabel('target x-velocity'); x-velocity'); title('Rad title ('Radar ar based target tracking tracking using the Kalm Kalman an filter'); filter'); legend('actual','estimated');
Listing 3.2: target_tracking.m target_tracking.m
29
30 3. THE KA KALM LMAN AN FIL FILTE TER R 2
actual estimated 1.8
1.6
y t i c o l e v x1.4 t e g r a t
1.2
1
0.8 0
10
20
30
40
50
60
70
target x-position
Figure 3.2: Rada Radar-ba r-based sed targ target et tra tracki cking ng usi using ng the Kal Kalman man filt filter er.. e tar target get sta starts rts at x-p x-posi ositio tion n x0 D 1 with x-velocity xP 0 D 1 and moves according the dynamical law given in (3.7 ( 3.7), ), with t D 0:5 and Nois isyy ran range ge an and d ra rang ngee-rat ratee me meas asur urem emen ents ts ar aree co coll llec ecte ted d as sp spec ecifi ified ed in (3.11 3.11), ), wi with th y D 0:1 . x D 0:1 . No N D D 100 time steps of Kalman filtering are performed and the actual and estimated target position and velocity are shown in the plot.
and the state noise vector vn D Œv n;1 vn;2 vn;3 vn;4 T is 4 1 and Gaussian with zero mean and covariance matrix 2 x
D
Qn
2 66 4
.t/3 3
0 .t/2 2
0
0
.t/3 3
.t/2 2
0
0
.t/2 2
0
t
0
.t/2 2
0
t
3 77 5
:
(3.14)
e measurement process collects information about the target’s range r n , range-rate Prn , and bearing angle n , and is now nonlinear and of the form rn
D rP D n
n
D
q C C p P C C P C xn2 xn xn
xn2
tan1
yn2 wn;1 ; yn yn wn;2 ; yn2 yn wn;3 ; xn
C
(3.15a)
(3.15b) (3.15c)
3.3.. EX 3.3 EXAMP AMPLE LES S
31
where the radar position is taken as the origin .0; .0; 0/, and w n;1 , wn;2 , and w n;3 are i.i.d. Gaussian 2 with zero mean and variance y . In matrix notation, xn yn xn yn
2 3 0B26 371C 2 3 4 P 5 D B@64 P 75CA C 4 5 rn rn n
h
wn;1 wn;2 wn;3
P
:
(3.16)
us, the measurement vector yn D Œr n rPn n T is three-dimensional ( M D D 3), the function 4 3 describes the nonlinear nonlinear measureme measurement-sta nt-state te relationsh relationship, ip, and the measureme measurement nt noise h W R 7! R describes T vector wn D Œw n;1 wn;2 wn;3 is 3 1 and Gaussian with zero mean and covariance matrix Rn D y2 I . Due to the nonlinearity of the measurement model, the Kalman filter cannot be applied to estimate the moving target’s position and velocity. However, techniques such as the extended Kalman filter [6 [ 6, 12 12]] discussed in Section 3.1 Section 3.1 can can be used to solve this estimation problem.
3.3.2 CHA 3.3.2 CHANNE NNEL L EST ESTIMA IMATIO TION N IN CO COMMU MMUNIC NICA ATIO TIONS NS SYS SYSTEM TEMS S In wireless communications systems operating in an urban environment, signals from the transmitter may not reach the receiver directly due to scattering. Typically, the received signal is a superposition of the direct (line-of-sight) arrival and several reflected, phase-shifted, and delayed signals. is effect is known as multipath propagation. Figure 3.3 depicts a multipath wireless communications channel in an urban environment. Multipath channels are commonly modeled as a tapped-delay line (finite impulse response or FIR digital filter). In orthogonal frequencydivision multiplexing (OFDM) [21 [ 21]] systems, the time-varying properties of wireless communications channels have to be estimated. In the context of the Kalman filter, this problem can be posed as a system identification problem of estimating the FIR coefficients of the channel, measured by transmitting and receiving a discrete-time test signal filtered by the channel. A state space model describing the system can be obtained as follows. Let the time-varying filter coefficients of the channel evolve according to an autoregressive process, given by an;d
D ˛
d a n1;d
C v
n;d ;
d
D D 1;2; 3;
(3.17)
where an;1 , an;2 , an;3 are the channel coefficients at time step n (the FIR filter is of order 3), ˛ 1 , ˛2 , ˛ 3 are the autoregressive parameters, and v n;1 , v n;2 , v n;3 are i.i.d. Gaussian with zero mean and variance x2 . In matrix notation,
2 3 2 4 5 D 4 an;1 an;2 an;3
˛1 0 0
0 ˛2 0
0 0 ˛3
32 54
an1;1 an1;2 an1;3
3 2 3 5C4 5 vn;1 vn;2 vn;3
:
(3.18)
32 3. THE KA KALM LMAN AN FIL FILTE TER R
Figure 3.3: A multipath wireless communications channel in an urban environment. e received signal is a superposition of the direct arrival and several reflected, phase-shifted, and delayed signals. e multipath channel is modeled as an FIR filter, filter, whose coefficients can be estimated by transmitting and receiving a test signal through the channel.
us, the state vector x n D Œa n;1 an;2 an;3 T is three-dimensional ( D D 3 ), the state-transition matrix F n is 3 3 and given by
2 D 4
Fn
˛1 0 0
0 ˛2 0
0 0 ˛3
3 5
;
(3.19)
and the state noise vector v n D Œv n;1 vn;2 vn;3 T is 3 1 and Gaussian with zero mean and co variance matrix Q n D x2 I . e noisy measurements are obtained as the filtered output for a test signal u n propagated through the channel as 3
X D
yn
d D1
an;d u nd C1
Cw ; n
(3.20)
3.3.. EX 3.3 EXAMP AMPLE LES S
33
where w n is Gaussian with zero mean and variance y2 . In matrix notation,
D
yn
un
un1
un2
2 3 4 5C an;1 an;2 an;3
wn :
(3.21)
us, the measurement vector yn D y n is one-dimensional ( M D D 1 ), the measurement matrix T measurement nt noise vector vector w n D w n is scalar and Hn D u n D Œu n un1 un2 is 1 3, and the measureme 2 Gaussian with zero mean and variance R n D y . e state evolution and measurement models are both linear and Gaussian, and the Kalman filter can be used to estimate the time-varying channel coefficients. e MATLAB program D 500 time steps of channel estimachannel_estimation.m shown in Listing 3.3 Listing 3.3 performs performs N D tion using the Kalman filter. e state space model parameters were α D Œ0:85 Œ0:85 1:001 1:001 0:95T ,
D 0:1, u i.i.d. N .u I 0;1/, D 0:1, the initial channel coefficients were x N .x I 0; I/, and the Kalman filter was initialized with Gaussian mean m j D x and covariance P j D I .
x
n
n
y
0
00
0
0
00
e MATLAB MATLAB function kalman_filter.m shown in Listing 3.1 3.1 is is utilized to carry out the Kalman filter prediction and update steps (Eq. ( 3.5 3.5)). )). Figure 3.4c Figure 3.4c shows shows plots of the actual and estimated channel coefficients. It can be seen that the Kalman filter is able to estimate the time varying channel coefficients with good accuracy. accuracy. 1
% Channe Channel l Estima Estimatio tion n in Commun Communica icatio tion n System Systems s using using the Kalman Kalman Filter Filter
2 3 4 5 6 7 8 9 10
% State State space space mode model l alpha alpha = [0.8 [0.85; 5; 1.00 1.001; 1; -0.9 -0.95]; 5]; F_n = diag(alpha); sigma_x = 0.1; Q_n Q_n = sigm igma_x a_x^2*e ^2*ey ye(3) e(3); ; u = randn(501,1); sigma_y = 0.1; R_n = sigma_y^2;
% % % % % % %
Autor Autoregr egress essive ive parame parameter ters s State-transition matrix State noise standard deviation Stat tate noise oise cova covar rianc iance e matr atrix Test signal Measurement noise standard deviation Measurement noise covariance matrix
11 12 13 14 15
% Initiali Initializatio zation n x(1: x(1:3, 3,1) 1) = rand randn( n(3, 3,1) 1); ; m(1: m(1:3 3,1) ,1) = x(1: x(1:3, 3,1 1); P(1: P(1:3, 3,1: 1:3, 3,1) 1) = eye eye(3 (3); );
% Init Initia ial l chan channe nel l coef coeffi fici cien ents ts % Gau Gaussia ssian n post poster eri ior mean mean at time ime step step 1 % Gau Gauss ssia ian n pos poste teri rior or cova covari rian ance ce at time time step step 1
16 17 18
% Channe Channel l Estima Estimatio tion n using using the Kalman Kalman filter filter for n = 2 : 500, % Time steps
19 20 21
% State State propagat propagation ion v_n = sigma_x*randn(3,1);
% State noise vector
34 3. THE KA KALM LMAN AN FIL FILTE TER R 22
x(1: x(1:3, 3,n) n) = F_n* F_n*x( x(1: 1:3, 3,nn-1) 1) + v_n; v_n;
% Mark Markov ov line linear ar Gaus Gaussi sian an evol evolut utio ion n
% Generate Generate measureme measurements nts H_n = u(n+1:-1:n-1,1)'; w_n = sigma_y*randn(1,1); y_n = H_n*x(1:3,n) + w_n;
% Measurement matrix % Measurement noise vector % Linear Gaussian measurements
23 24 25 26 27 28 29 30 31 32
% Compu Compute te Gaus Gaussia sian n poster posterior ior mean mean and covari covarianc ance e at time time step step n [m(1:3,n),P(1:3,1:3,n)] [m(1:3,n),P(1:3,1:3,n)] = kalman_filter (m(1:3,n-1),... P(1:3,1:3,n-1),y_n,F_n,Q_n,H_n,R_n); end
33 34 35 36 37 38 39 40 41 42 43
% Plot Plot actual actual and estima estimated ted channel channel coeffic coefficien ients ts n = [0:499]'; [0:499]'; figure, figure, plot(n,x( plot(n,x(1,:), 1,:),'b'); 'b'); hold on, plot(n,m(1 plot(n,m(1,:),' ,:),'r--' r--'); ); xlabel('time'); ylabel('coefficie ylabel('coefficient nt 1'); legend('actual','estimated') legend('actual','estimated'); ; title('Cha title('Channel nnel Estimation Estimation in Communica Communication tion Systems using Kalman Filter'); Filter'); figure, figure, plot(n,x(2 plot(n,x(2,:),' ,:),'g'); g'); hold on, plot(n,m( plot(n,m(2,:), 2,:),'m--' 'm--'); ); xlabel('time'); ylabel('coefficie ylabel('coefficient nt 2'); legend('actual','estimated') legend('actual','estimated'); ; title('Cha title('Channel nnel Estimation Estimation in Communica Communication tion Systems using Kalman Filter'); Filter'); figure, figure, plot(n,x(3 plot(n,x(3,:),' ,:),'k'); k'); hold on, plot(n,m( plot(n,m(3,:), 3,:),'c--' 'c--'); ); xlabel('time'); ylabel('coefficie ylabel('coefficient nt 3'); legend('actual','estimated') legend('actual','estimated'); ; title('Cha title('Channel nnel Estimation Estimation in Communica Communication tion Systems using Kalman Filter'); Filter');
Listing 3.3: channel_estimation.m channel_estimation.m
3.3.3 REC 3.3.3 RECUR URSIV SIVE E LE LEAST AST SQU SQUAR ARES ES (R (RLS LS)) AD ADAPTI APTIVE VE FIL FILTER TERING ING We We consider the adaptive filtering problem problem where the goal is to design a digital digital filter that operates linearly on an input signal such that the filtered output closely approximates a desired signal of interest. e filter constantly adapts in time based on the statistics of the given signals, which can be non-stationary (in the stationary case, the optimum filter is time-invariant and given by the Wiener filter as shown in Section 2.2 Section 2.2). ). Adaptive filters are used in many applications, including signal denoising, prediction, and system modeling and identification [ 12 12]. ]. e adaptive filtering problem can be formulated in the state space setting as follows. Assume that the time-varying time-var ying filter coefficients evolve according to a dynamical law law,, given by
D
an;d
1=2
an1;d ;
d
D D 1; 1; : : : ; D ;
(3.22)
3.3.. EX 3.3 EXAMP AMPLE LES S
35
1 actual estimated
0.5 1 t n e i c
ffi e o c
0
−0.5 0
100
200
300
400
500
time
(a) Estimation of channel coefficient 1
Chann annel el es esti tima mati tion on in co comm mmuni unicat catio ion n sy syst stem emss us usin ingg th thee Kal Kalma man n fil filte terr. e Figure 3.4a: Ch D D 3 channe channell co coeffi effici cien ents ts ev evol olve ve ac acco cord rdin ingg th thee dyn dynam amica icall la law w gi give ven n in ( 3.18 3.18), ), wi with th α D T Œ0:85 1:001 0:95 and x D 0:1 . Noisy measurements are collected as specified in (3.21 ( 3.21), ), with un
i.i.d.
N .un 0;1/ and y
I
D 0:1. N D 500 time steps of Kalman filtering are performed and the ac-
tual and estimated channel coefficients are shown in the plots.
36 3. THE KA KALM LMAN AN FIL FILTE TER R
1 actual estimated
0.5 0 −0.5 2 −1 t n e i c−1.5
ffi e o c
−2
−2.5 −3 −3.5 −4 0
100
200
300
400
500
time
(b) Estimation of channel coefficient 2
Chann annel el es esti tima mati tion on in co comm mmun unic icat atio ion n sy syst stem emss us usin ingg th thee Kal Kalma man n fil filte terr. e Figure 3.4b: 3.4b: Ch D D 3 channe channell co coeffi effici cien ents ts ev evol olve ve ac acco cord rdin ingg th thee dyn dynam amica icall la law w gi give ven n in ( 3.18 3.18), ), wi with th α D T Œ0:85 1:001 0:95 and x D 0:1 . Noisy measurements are collected as specified in (3.21 ( 3.21), ), with un
i.i.d.
N .un 0;1/ and y
I
D 0:1. N D D 500 time steps of Kalman filtering are performed and the ac-
tual and estimated channel coefficients are shown in the plots.
3.3.. EX 3.3 EXAMP AMPLE LES S
37
1 actual estimated
0.8 0.6 0.4 3 t n e i c
ffi
0.2 0
e o c−0.2
−0.4 −0.6 −0.8 −1 0
100
200
300
400
500
time
(c) Estimation of channel coefficient 3
Chan anne nell es esti tima mati tion on in co comm mmuni unica cati tion on sy syst stem emss us usin ingg th thee Kal Kalma man n fil filte terr. e Figure 3.4c: 3.4c: Ch D D 3 channe channell co coeffi effici cien ents ts ev evol olve ve ac acco cord rdin ingg th thee dyn dynam amica icall la law w gi give ven n in ( 3.18 3.18), ), wi with th α D T Œ0:85 1:001 0:95 and x D 0:1 . Noisy measurements are collected as specified in (3.21 ( 3.21), ), with un
i.i.d.
N .un 0;1/ and y
I
D 0:1. N D 500 time steps of Kalman filtering are performed and the ac-
tual and estimated channel coefficients are shown in the plots.
38 3. THE KA KALM LMAN AN FIL FILTE TER R
where a n;1 ; an;2 ; : : : ; an;D are the filter coefficients at time step n (the filter is FIR and of order D ) and 2 .0; .0; 1 is an exponential weight parameter. In matrix notation,
2 66 4
an;1 an;2 :: : an;D
3 77 D 5
1=2 I
2 66 4
an1;1 an1;2 :: : an1;D
3 77 5
:
(3.23)
us, the state vector xn D Œa n;1 an;2 : : : an;D T is D-dimen -dimensio sional nal,, and the statestate-tra transi nsitio tion n ma1=2 trix F n D I is D D . e evolution model is deterministic and the state noise covariance matrix is Q n D 0 . e measurement model relates the input signal u n and filtered output y n as D
X D
yn
an;d u nd C1
d D1
Cw ; n
(3.24)
where w n is Gaussian with zero mean and unit variance ( y2 D 1 ). In matrix notation,
yn
D
un
un1 : : :
2 664
unD C1
an;1 an;2 :: : an;D
3 77 C 5
wn :
(3.25)
us, the measurement vector yn D y n is one-dimensional ( M D D 1), the measurement matrix T Hn D u n D Œu n un1 : : : unD C1 is 1 D , and the measurement noise vector wn D w n is scalar and Gaussian with zero mean and variance R n D y2 D 1 . Observe the similarity of this state space model to that used in the channel estimation system identification example of Subsection 3.3.2 3.3.2.. For this model, the Kalman filter equations ( 3.5 3.5)) can be simplified to kn mnjn P njn
P n1jn1 un
; uT n P n1jn1 un 1=2 mn1jn1 kn .yn 1=2 uT n mn1jn1 /; T 1 1 P n1jn1 kn un P n1jn1 ;
D D D
C C
(3.26a)
(3.26b) (3.26c)
for n D 1; 1; 2 ; : : : . In particular, the substitution mnjn D n=2 aO n (adaptive filter) and yn D n=2 sn (desired signal) yields D u P P j uu C ; j O D aO C k .s u Oa /; a D P j k u P j ; P j kn n
nn
n 1n 1
T n
n 1 1
(3.27a)
n
n 1n 1 n
n
n
n 1n 1
T n 1
n 1 T n n
n 1n 1
(3.27b) (3.27c)
3.3.. EX 3.3 EXAMP AMPLE LES S
39
for n D 1; equations [12 [12,, 1; 2 ; : : : , which are also known as the recursive least squares (RLS) filtering equations 22––26 22 26]. ]. Figure 3.5 Figure 3.5 shows shows a block diagram of the adaptive filtering framework.
Figure 3.5: Block diagram of the adaptive filtering framework. e filter operates linearly on an input signal and constantly adapts such that the filtered output closely matches the given measurements.
e state evolution and measurement models are linear and Gaussian, and the Kalman filter (or, equivalently, the RLS filter) can be used to estimate the time-varying filter coefficients. D 500 time steps e MATLAB MATLAB program rls_filtering.m shown in Listing 3.4 Listing 3.4 performs performs N D of estimation using the Kalman filter. e state space model parameters were D D 32 , D 0:99, i.i.d.
N .un 0; 100/ un 100/, y 1 (these values were selected similar to the example in MATLAB’s [ 27]), ]), and the Kalman filter was initialized with Gausadaptfilt.rls function documentation [27 sian sian mea mean n m0j0 0 and covariance covariance P 0j0 I . e MATLA MATLAB B functi function on kalman_filter.m shown
I
D
D
D
in Listing 3.1 Listing 3.1 is is utilized to carry out the Kalman filter prediction and update steps (Eq. ( 3.5 3.5)). )). Figure 3.6 3.6 shows shows a plot of the actual and estimated filter coefficients. It can be seen that the Kalman filter is able to estimate the time-varying filter coefficients with good accuracy. e Kalman filter described in this chapter can be applied for parameter estimation of dynamical systems that may be represented using a linear Gaussian state space model. Several extensio tensions ns to the standar standard d setup setup are are possib possible, le, includin includingg estima estimatio tion n algori algorithm thmss for nonlin nonlinear ear system systems, s, systems with non-Gaussian noise, and distributed systems involving data collected from multi-
40 3. THE KA KALM LMAN AN FIL FILTE TER R 1
% Rec Recurs ursive ive least squares squares (RL (RLS) S) ada adapti ptive ve fil filter tering ing using the Kal Kalman man filter filter
2 3 4 5 6 7 8 9 10
% State State space space mode model l D = 32; lambda = 0.99; F_n F_ n = 1/ 1/sq sqrt rt(l (lam ambd bda) a); ; Q_n = zeros(D,D); u = 10 10*r *ran andn dn(5 (500 00+D +D-2 -2,1 ,1); ); sigma_y = 1; R_n = sigma_y^2;
% % % % % % %
% Initiali Initializatio zation n x(1:D, x(1 :D,1) 1) = fir1 fir1(D(D-1,0 1,0.5) .5); ; m(1: m( 1:D, D,1) 1) = zer zeros os(D (D,1 ,1); ); P(1: P( 1:D, D,1: 1:D, D,1) 1) = eye eye(D (D); );
% Ini Initia tial l filter filter coef coeffic ficien ients ts % Gau Gauss ssia ian n pos poste teri rior or me mean an at ti time me st step ep 1 % Gau Gauss ssia ian n p pos oste teri rior or co cova vari rian ance ce at ti time me st step ep 1
Order of FIR filter Exponential weight parameter Stat St atee-tr tran ansi siti tion on ma matr trix ix State noise covariance matrix Input Inp ut si sign gnal al Measurement noise standard deviation Measurement noise covariance matrix
11 12 13 14 15 16 17 18
% RLS fil filter tering ing using using the Kalman Kalman filt filter er for n = 2 : 500, % Time steps
19 20 21
% Stat State e prop propagati agation on x(1:D,n) = F_n*x(1:D,n-1);
% Markov linear evolution
% Gene Generate rate measu measureme rements nts H_n = u(n+D-2:-1:n-1,1)'; w_n = sigma_y*randn(1,1); y_n = H_n*x(1:D,n) + w_n;
% Measurement matrix % Measurement noise vector % Linear Gaussian measurements
22 23 24 25 26 27 28 29 30 31
% Compu Compute te Gaus Gaussia sian n poster posterior ior mea mean n and cov covari arianc ance e at time time s step tep n [m(1:D,n),P(1:D,1:D,n)] [m(1:D,n),P(1: D,1:D,n)] = kalman_filter (m(1:D,n-1),... P(1:D,1:D,n-1),y_n,F_n,Q_n,H_n,R_n); end
32 33 34 35 36 37
% Plo Plot t act actual ual and est estima imated ted filter filter coe coeffi fficie cients nts figure, figur e, stem( stem(x(1:D x(1:D,500) ,500),'b') ,'b'); ; hold on, stem( stem(m(1:D m(1:D,500) ,500),'r-,'r--'); '); xlabel('coefficient xlabel('coeffici ent number'); ylabel('filter ylabel('filter coefficient'); coefficient'); title('RLS title ('RLS adaptive adaptive filtering filtering using the Kalman filter'); filter'); legend('actual','estimated');
Listing 3.4: rls_filtering.m rls_filtering.m
3.3.. EX 3.3 EXAMP AMPLE LES S
41
6 actual estimated
5
4
t n e i c
3
e o c r e t l
2
ffi
fi 1 0
−1
−2 0
5
10
15
20
25
30
coeffi coe fficient number
Figure 3.6: RLS adaptive filtering using the Kalman filter. e D D 32 order FIR filter coefficients evolve according the dynamical law given in (3.23 ( 3.23), ), with D 0:99. Noisy measurements are collected i.i.d.
100// and y D 1 . N D as specified in (3.25 (3.25), ), with un N .un I 0; 100 D 500 time steps of Kalman filtering are performed and the actual and estimated filter coefficients at the final time step are shown in the plot.
ple sources. In the following chapter, we will discuss the extended Kalman filter for nonlinear problems, as well as a decentralized formulation of the Kalman filter.
43
CHAPTER 4
Exte Ex tend nded ed an and d De Dece cent ntra rali lized zed Kalm Ka lman an Fil Filte terin ring g We We complete our discussion of Kalman filtering with an outline of two popular extensions de veloped for nonlinear problems and distributed applications. Nonlinear state space models are often encountered, for example, in target tracking [ 6] and inertial navigation systems [8 [ 8, 9 9]. ]. Distributed estimation tasks arise frequently in the context of sensor networks [ 28 28]] and multisensor tracking [29 [29,, 30 30]. ]. In this chapter we examine the extended and decentralized Kalman filters and illustrate their utility through examples.
4.11 4.
EXTE EX TEND NDE ED KA KALM LMAN AN FI FIL LTE TER R
When the state evolution and measurement processes are nonlinear and Gaussian, the extended [ 6, 12 12]] can be utilized for estimation of the states. e state space model for this Kalman filter [6 case can be written as xn yn
f .xn1 / vn ; h.xn / wn ;
D D
C
C
(4.1a) (4.1b)
where f W RD 7! RD is the state-transition function, h W RD 7! RM is the measurement measurement function, and v n and w n are independent Gaussian noise vectors as defined earlier. Assuming that f and h are differentiable, the model can be linearized by making use of the Jacobian matrices:
Q D
Fn
Q
Hn
D
@f @x
@h @x
ˇˇ ˇ
2 6 6 D6 4 2 6 6 D6 4
@f 1 @x1 @f 2 @x1
@f 1 @x2 @f 2 @x2
:::
@f D @x1
@f D @x2
:::
:: :
mn1jn1
ˇˇˇ
mnjn1
:: :
::: :: :
@h1 @x1 @h2 @x1
@h1 @x2 @h2 @x2
:::
@hM @x1
@hM @x2
:::
:: :
:: :
::: :: :
@f 1 @xD @f 2 @xD
:: :
@f D @xD @h1 @xD @h2 @xD
:: :
@hM @xD
3ˇˇ 77ˇ 75ˇˇ ˇ3ˇ 777ˇˇˇ 5ˇˇ
;
(4.2a)
mn1jn1
mnjn1
:
(4.2b)
44
4. EXTE EXTENDE NDED D AND DEC DECENT ENTRAL RALIZE IZED D KAL KALMAN MAN FIL FILTER TERING ING
e extended Kalman filter equations are given by [6 [ 6, 12 12]] mnjn1 P njn1 Sn Kn mnjn P njn
D f .m j /; Q Q D F P j F C Q ; D HQ P j QH C R ; D P j QH S ; D m j C K .y h.m j //; D P j K QH P j ; n 1n 1
T n n 1n 1 n T n nn 1 n T 1 nn 1 n n n
nn 1
n
n
n
nn 1
n
n
nn 1
nn 1
(4.3a) (4.3b) (4.3c) (4.3d) (4.3e) (4.3f)
for n D 1; ( 4.3a)–( )–(4.3b 4.3b)) representing the prediction step and Eqs. ( 4.3c 4.3c)–( )–(4.3f 4.3f ) 1; 2 ; : : : , with Eqs. (4.3a comprising the update step. Note that the extended Kalman filter equations ( 4.3 4.3)) reduce to the Kalman filter equations (3.5 ( 3.5)) when the state-transition function f and measurement function h are linear. e extended Kalman filtering algorithm is summarized below. Algorithm 2 Extended Kalman filtering ( 4.1), ), and a set of measurements fy1 ; y2 ; : : : ; yN g. Input : Nonlinear Gaussian state space model (4.1 1;; 2 ; : : : ; N . Output : Estimates of the states x 1 ; x2 ; : : : ; xN at time steps n D 1 Initialize the mean and covariance of the Gaussian state distribution N .x0 I m0j0 ; P 0j0 / at time step n D 0 . For time steps n D 1 1;; 2 ; : : : ; N , perform the following: 1. Carry out the predict prediction ion step mnjn1 P njn1
D f .m j /; D FQ P j QF C Q : n 1n 1
n
n 1n 1
T n
n
2. Comp Compute ute the estimat estimatee Oxn D m njn of the state x n using the update step Sn Kn mnjn P njn
T n 1 n
D HQ P j QH C R ; D P j QH S ; D m j C K .y h.m j //; D P j K QH P j : n
nn 1 T nn 1 n nn 1
nn 1
n
n
n
n
n
nn 1
nn 1
e MATLAB MATLAB function extended_kalman_filter.m shown shown in Listin Listingg 4.1 per perfor forms ms one iteration of the extended Kalman filter prediction and update steps. Other algorithms for nonlinear and/or non-Gaussian filtering include those based on unscented transforms [12 [12,, 31 31]] and Monte Carlo techniques such as the particle filter [ 5, 16 16]. ].
4.1.. EX 4.1 EXTEN TENDED DED KAL KALMAN MAN FIL FILTER TER 45
1 2
function [m_n,P_n] = extended_kalman_f extended_kalman_filter ilter (m_n1,P_n1,y_n,f,Ft_n,Q_n.. (m_n1,P_n1,y_n,f,Ft_n,Q_n... . ,h,Ht_n,R_n)
3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21
% % % % % % % % % % % % % % % % % %
Extended Extend ed Kal Kalman man filter filter pre predic dictio tion n and update update ste steps ps --- propagat propagates es Gaussi Gau ssian an posterio posterior r sta state te distribu distributio tion n fro from m tim time e ste step p n-1 to tim time e ste step p n (for (f or detai details ls, , se see e Se Sect ctio ion n 4. 4.1 1 of the text) text) Inputs: Inputs : m_n1 m_ n1 - (D (Dx1 x1 ve vect ctor or) ) Ga Gaus ussi sian an po post ster erio ior r me mean an at ti time me st step ep nn-1 1 P_n1 P_n 1 - (Dx (DxD D mat matrix rix) ) Gau Gaussi ssian an posterio posterior r cov covari arianc ance e at time step n-1 y_n y_ n - (M (Mx1 x1 vecto vector) r) measu measure reme ment nts s at time time st step ep n f(x) f(x ) - (Dx (Dx1 1 vec vector tor functio function n of Dx1 vector vector x) sta statete-tra transi nsitio tion n fun functi ction on Ft_n Ft_ n - (Dx (DxD D mat matrix rix) ) sta statete-tra transi nsitio tion n Jac Jacobi obian an matrix matrix at time step n Q_n Q_ n - (D (DxD xD ma matr trix ix) ) Ga Gaus ussi sian an state state no nois ise e co cova vari rian ance ce at ti time me step n h(x) h(x ) - (Mx (Mx1 1 vec vector tor functio function n of Dx1 vec vector tor x) mea measur sureme ement nt function function Ht_n Ht_ n - (Mx (MxD D mat matrix rix) ) mea measur sureme ement nt Jacobia Jacobian n mat matrix rix at time step n R_n - (Mx (MxM M mat matrix rix) ) Gau Gaussi ssian an measurem measurement ent noise noise cov covari arianc ance e at time step n Outputs: Outputs: m_n m_ n - (D (Dx1 x1 vector vector) ) Gaus Gaussi sian an poste posteri rior or mea mean n at time time st step ep n P_n - (Dx (DxD D mat matrix rix) ) Gau Gaussi ssian an posteri posterior or covaria covariance nce at time step n
22 23 24 25
% Pre Predic dict t m_nn1 = feval feval(f,m_ (f,m_n1); n1); P_nn1 P_n n1 = Ft_ Ft_n*P n*P_n1 _n1*Ft *Ft_n' _n' + Q_n Q_n; ;
26 27 28 29 30 31
% Upd Update ate S_n = Ht_ Ht_n*P n*P_nn _nn1*H 1*Ht_n t_n' ' + R_n R_n; ; K_n = P_nn1*H P_nn1*Ht_n'/ t_n'/S_n; S_n; m_n = m_nn m_nn1 1 + K_n* K_n*(y_n(y_n-feval feval(h,m_ (h,m_nn1)) nn1)); ; P_n = P_n P_nn1 n1 - K_n K_n*Ht *Ht_n* _n*P_n P_nn1; n1;
Listing 4.1: extended_kalman_filter.m extended_kalman_filter.m
46
4. EXTE EXTENDE NDED D AND DEC DECENT ENTRAL RALIZE IZED D KAL KALMAN MAN FIL FILTER TERING ING
4.1.1 EX 4.1.1 EXAMPL AMPLE: E: PRE PREDA DATOR TOR-PREY -PREY SYSTE SYSTEM M We We consider a bio-system occupied by a predator population and a prey population. population. is type of a system is modeled using the Lotka-Volterra equations [ 32 32]. ]. In this model, the predator and prey interact based on the following equations: xn;1 xn;2
D D
C 1
t 1
1
t 1
xn1;2 c2 xn1;1 c1
xn1;1 xn1;2
n;1 ;
(4.4a)
n;2 :
(4.4b)
Cv Cv
Here, x n;1 and x n;2 denote respectively the prey and predator populations at time step n , t is the time interval between time steps n 1 and n , c 1 and c 2 are constant model parameters, and vn;1 and vn;2 are i.i.d. Gaussian with zero mean and variance x2 . Clearly, the two populations do not live in isolation. For example, if left with an infinite food supply and no predators, a population of rabbits would experience normal population growth. Similarly, a population of foxes with no rabbit population population would would face decline. When together together,, these populations populations interact interact according according to (4.4 ( 4.4). ). Figure 4.1 Figure 4.1 depicts depicts the cycle of predator-prey population dynamics.
Figure 4.1: Cycle of predator-prey population dynamics. When there is an abundance of preys, the predator population increases. But this increase in predators drives the prey population down. With a scarcity of preys, the predator population is forced to decrease. e decrease in predators then results in prey population increasing, and the cycle continues.
4.1.. EX 4.1 EXTEN TENDED DED KAL KALMAN MAN FIL FILTER TER 47
In matrix notation, (4.4 ( 4.4)) can be expressed as
D xn;1 xn;2
xn1;1 xn1;2
f
C vn;1 vn;2
:
(4.5)
e state vector xn D Œx n;1 xn;2 T is two-dimensional ( D D 2 ), f W T W R2 7! R2 is the nonlinear state-transition function, and the state noise vector vn D Œv n;1 vn;2 is 2 1 and Gaussian with zero mean and covariance matrix Q n D x2 I . We We wish to estimate the predator and prey populations based on noisy measurements of the total population (4.6) yn D x n;1 C xn;2 C wn ; where w n is Gaussian with zero mean and variance y2 . In matrix notation,
D C
yn
1
xn;1 xn;2
1
wn :
(4.7)
D 1 ), the measurement matrix us, the measurement vector yn D y n is one-dimensional ( M D Hn D Œ1 1 is 1 2, and the measurement noise vector wn D w n is scalar and Gaussian with zero mean and variance R n D y2 . e state space model above is nonlinear and Gaussian, and the extended extended Kalman filter can be used for estimation. In particular, the state-transition function Jacobian matrix (Eq. ( 4.2a 4.2a)) )) is 2 2 and given by
2 Q D 4 D
Fn
1
C t
t
1
mn1jn1;2 c2
mn1jn1;2 c1 T
m t n1cjn1;1 2 mn1jn1;1 t 1 c1
1
3 5
;
(4.8)
where m n1jn1 mn1jn1;1 mn1jn1;2 is the mean of the Gaussian posterior pdf at time step n 1. e MATLAB MATLAB program predator_prey.m shown shown in Listin Listingg 4.2 performs N D D 2000 time steps of predator-prey population estimation using the extended Kalman filter. e state space model parameters were t D 0:01, c1 D 300 , c2 D 200 , x D 1 , y D 10 , the initial prey and predator populations were set as x 0 D Œ400 100 T (these values were selected to generate a plot simi simila larr to thatin thatin [33 33,, Ch. Ch. 16]) 16]),, and and the the exte extende nded d Kalma Kalman n filte filterr was was init initia iali lized zed with with Gaus Gaussi sian an me mean an extended_kalman_filter.m m0j0 D x 0 and covariance P 0j0 D 100 I . e MATLAB function extended_kalman_filter.m shown in Listing 4.1 Listing 4.1 is utilized to carry out the extended Kalman filter prediction and update steps (Eq. (4.3 (4.3)). )). Figure 4.2b Figure 4.2b shows shows a plot of the actual and estimated predator-prey populations. It can be seen that the extended Kalman filter is able to track the predator-prey dynamics with good accuracy.
48 1
4. EXTE EXTENDE NDED D AND DEC DECENT ENTRAL RALIZE IZED D KAL KALMAN MAN FIL FILTER TERING ING
% Soluti Solution on of Predat Predatoror-Pre Prey y Equati Equations ons using the Extend Extended ed Kalman Kalman Filter Filter
2 3
function predator_prey
4 5 6 7 8 9 10 11 12
% State State space space mode model l delt = 0.01; c1 = 300; c2 = 200; si sigma_x = 1; Q_n Q_n = sigm sigma_ a_x^ x^2* 2*ey eye( e(2) 2); ; H_n = [1 1]; sigma_y = 10; R_n = sigma_y^2;
% % % % % % %
Time step interval Predator-Prey model parameters State noise standard deviation Stat State e nois noise e cova covari rian ance ce matr matrix ix Measurement matrix Measurement noise standard deviation Measurement noise covariance matrix
13 14 15 16 17
% Initiali Initializatio zation n x(1: x(1:2, 2,1) 1) = [40 [400; 0; 100] 100]; ; m(1 m(1:2,1 :2,1) ) = x(1: x(1:2, 2,1 1); P(1:2, P(1:2,1:2 1:2,1) ,1) = 100*eye( 100*eye(2); 2);
% Ini Initi tial al prey prey and and pre preda dato tor r pop popul ulat atio ions ns % Gauss aussi ian post poster eri ior mean mean at time time step step 1 % Gaussi Gaussian an posteri posterior or covaria covariance nce at time step 1
18 19 20
% Predator Predator-Prey -Prey estimation estimation using using extended Kalman Kalman filter for n = 2 : 2000, % Time steps
21 22 23 24
% State State propagati propagation on v_n = sig sigma_x ma_x* *ran randn(2 dn(2, ,1); 1); x(1:2, x(1:2,n) n) = f(x(1: f(x(1:2,n 2,n-1) -1)) ) + v_n; v_n;
% Stat State e nois oise vec vector tor % Marko Markov v nonli nonlinea near r Gauss Gaussian ian evolut evolution ion
% Generate Generate measureme measurements nts w_n = sig sigma_y ma_y* *ran randn(1 dn(1, ,1); 1); y_n = H_n H_n*x(1 *x(1: :2,n 2,n) + w_n; _n;
% Meas Measur ure emen ment noi noise vect vector or % Line Linear ar Gau Gaussia ssian n meas easurem ureme ents nts
25 26 27 28 29 30 31 32
% State-tra State-transiti nsition on function function Jacobian Jacobian matrix matrix Ft_n = [1+delt*(1-m(2,n-1)/c2) [1+delt*(1-m(2,n-1)/c2) -delt*m(1,n-1)/c2;... -delt*m(1,n-1)/c2;... delt*m(2,n-1)/c1 delt*m(2,n-1)/c1 1-delt*(1-m(1,n-1)/c1)]; 1-delt*(1-m(1,n-1)/c1)];
33 34 35 36 37 38
% Compu Compute te Gaus Gaussia sian n poster posterior ior mean mean and covari covarianc ance e at time time step step n [m(1:2,n),P(1:2,1:2,n)] [m(1:2,n),P(1:2,1:2,n)] = extended_kalman_filter extended_kalman_filter (m(1:2,n-1),... (m(1:2,n-1),... P(1:2,1:2,n-1),y_n,@f,Ft_n,Q_n,@h,H_n,R_n); end
4.2.. DECENTR 4.2 DECENTRAL ALIZE IZED D KAL KALMAN MAN FIL FILTER TERING ING 49 39 40 41 42 43 44 45 46 47 48 49 50 51 52
% Plot actual actual and estimated estimated predator-p predator-prey rey populations populations % Evolut Evolution ion in in time time t = [0:199 [0:1999]'*d 9]'*delt; elt; figure, figure, plot(t,x(1 plot(t,x(1,:),' ,:),'b'); b'); hold on, plot(t,m( plot(t,m(1,:), 1,:),'r--' 'r--'); ); hold on, plot(t,x( plot(t,x(2,:), 2,:),'g.-' 'g.-'); ); hold on, plot(t,m plot(t,m(2,:) (2,:),'m-. ,'m-.'); '); xlabel('time'); ylabel('population'); ylabel('population'); title('Sol title('Solution ution of predatorpredator-prey prey equations equations using extended extended Kalman filter'); filter'); legend('actual preys','estimated preys','estimated preys','actual preys','actual predators',... predators',... 'estimated predators'); % Phas Phase e plot plot figure, plot(x(1,:),x(2,:),'b'); plot(x(1,:),x(2,:),'b'); hold hold on, plot(m(1,:),m(2,:),'r--'); plot(m(1,:),m(2,:),'r--'); xlabel('prey population'); population'); ylabel('predator ylabel('predator population'); population'); title('Sol title('Solution ution of predatorpredator-prey prey equations equations using extended extended Kalman filter'); filter'); legend('actual','estimated');
53 54 55 56 57 58
% State-trans State-transition ition functi function on func functi tion on x_n x_n = f (x_n1 (x_n1) ) x_n = [(1+delt*( [(1+delt*(1-x_n 1-x_n1(2,1 1(2,1)/c2) )/c2))*x_ )*x_n1(1 n1(1,1); ,1); ... (1-delt*(1-x_n1(1,1)/c1))*x_n1(2,1)]; end
59 60 61 62 63
% Measuremen Measurement t function function func functi tion on y_n y_n = h (x_n) (x_n) y_n = x_n(1, x_n(1,1) 1) + x_n(2, x_n(2,1); 1); end
64 65
end
Listing 4.2: predator_prey.m
4.2
DECEN DEC ENTRA TRALI LIZE ZED D KA KALMA LMAN N FIL FILTE TERIN RING G
In distributed multisensor network configurations, the decentralized Kalman filter [34 [ 34]] enables estimation for linear Gaussian state space models without the need for a central communication and processing facility. e resulting estimation framework is robust to failure of one or more nodes and can yield performance benefits such as faster processing capability. In this scheme, each sensing node first collects measurements about the system’s state and communicates a small amount of information with the other nodes. Next, each node locally processes the information received from the other nodes in conjunction with its own data to compute an overall estimate
50
4. EXTE EXTENDE NDED D AND DEC DECENT ENTRAL RALIZE IZED D KAL KALMAN MAN FIL FILTER TERING ING
700
actual preys estimated preys actual predators estimated predators
600
500
n o 400 i t a l u p o 300 p
200
100
0 0
5
10
15
20
time
(a) Evolution in time
Solution on of the Predator-Prey Predator-Prey equations equations using the extend extended ed Kalman filter. filter. e initi initial al Figure 4.2a: Soluti prey and predator populations are 400 and 100 , respectively, and evolve according the dynamical law given in (4.4 (4.4), ), with c1 D 300 and c2 D 200 . Noisy total population measurements are collected as D 2000 time steps of extended Kalman filtering are performed specified in (4.6 (4.6), ), with y D 10 . N D and the actual and estimated predator-prey populations are shown in the plots.
4.2.. DECENTR 4.2 DECENTRAL ALIZE IZED D KAL KALMAN MAN FIL FILTER TERING ING 51
400
actual estimated
300 n o i t a l u p o p r o t 200 a d e r p
100
100
200
300
400
500
600
prey population
(b) Phase plot
Solution on of the Predator-Prey Predator-Prey equations equations using the extended Kalman filter. e initi initial al Figure 4.2b: Soluti prey and predator populations are 400 and 100 , respectively, and evolve according the dynamical law given in (4.4 (4.4), ), with c1 D 300 and c2 D 200 . Noisy total population measurements are collected as D 2000 time steps of extended Kalman filtering are performed specified in (4.6 (4.6), ), with y D 10 . N D and the actual and estimated predator-prey populations are shown in the plots.
52
4. EXTE EXTENDE NDED D AND DEC DECENT ENTRAL RALIZE IZED D KAL KALMAN MAN FIL FILTER TERING ING
of the state. In the following brief description, it is assumed for simplicity that measurement validation is not used and that the communication between the nodes is synchronized; the reader is referred to [34 [ 34]] for the general method. Using Using matrix matrix invers inversion ion lem lemmas mas and some some algebr algebra, a, the Kalman Kalman filter filter update update equ equaations (3.5e (3.5e)–( )–(3.5f 3.5f ) for the mean mnjn and covariance P njn of the (Gaussian) posterior pdf p. xn jY n / at time step n can be rewritten in information form as P njn1 P njn1
P njn11
D D
mnjn
P njn11
T n
CH
1 R n Hn ;
mnjn1
C
HT n
1 R n
yn :
(4.9a) (4.9b)
Let K be the number of distributed nodes used to collect measurements, with the measurement equation for each of the nodes expressed as yn;k
D H
n;k x n
n;k ;
Cw
k
D 1; 1; : : : ; K ;
(4.10)
where y n;k denotes the M k 1 vector of measurements collected by sensor node k at time step n, H n;k is the M k D measurement matrix for node k , x n is the D 1 state vector, and w n;k is a M k 1 Gaussian random measurement noise vector with zero mean and covariance matrix Rn;k . In block matrix notation,
2 66 4
yn;1 yn;2 :: : yn;K
3 2 77 D 66 5 4
Hn;1 Hn;2 :: : Hn;K
3 77 5
xn
2 6 C 64
3 77 5
wn;1 wn;2 :: : wn;K
;
(4.11)
or
D H
yn
n
xn
Cw ; n
(4.12)
T T T where yn D Πyn;1 the comb combin ined ed M measure reme ment nt vect vector or ( M D : : : yn;K T is the 1 measu D yn;2 K T T T T D measurement matrix, and k D1 M k ), Hn D ΠHn;1 Hn;2 : : : Hn;K is the combined M T T T 1 Gaussian random measurement noise vec: : : wn;K T is the combined M wn D Πwn;1 wn;2 tor with zero mean and covariance matrix
P
Rn
2 6 D 64
Rn;1 0 :: : 0
0 Rn;2 :: : 0
::: ::: :: : :::
0 0 :: : Rn;K
3 77 5
:
(4.13)
4.2.. DECENTR 4.2 DECENTRAL ALIZE IZED D KAL KALMAN MAN FIL FILTER TERING ING 53
Here, the measurement noise vectors of the different sensor nodes have been assumed to be uncorrelated. Due to the block structure of the measurement model, we can write K
HT n
1 R n Hn
1 HT n Rn yn
D D
X X
T 1 Hn;k R n;k H n;k ;
(4.14a)
k D1 K T 1 Hn;k R n;k y n;k :
(4.14b)
k D1
Substituting into (4.9 (4.9)) yields K
P njn1 P njn1
mnjn
D
P njn11
D
P njn11
X C
T 1 Hn;k R n;k Hn;k ;
(4.15a)
k D1
mnjn1
K
X C
T 1 Hn;k R n;k y n;k ;
(4.15b)
k D1
which represent the decentralized Kalman filter update equations [ 34 34]. ]. e quantities inside the summation terms in (4.15 ( 4.15)) need to be communicated between the nodes in order to perform the state update step. Note that the decentralized Kalman filter update step ( 4.15 4.15)) is mathematically equivalent to the (centralized) Kalman filter update step in Eqs. (3.5c ( 3.5c)–( )–(3.5f 3.5f ); ); no approximation has been used. Assuming that the same global state evolution model of Eq. ( 3.1a 3.1a)) (and therefore the same prediction step in Eqs. (3.5a ( 3.5a)–( )–(3.5b 3.5b)) )) is used by all nodes, with identical initialization, the decentralized Kalman filter provides exactly the same state estimates as the centralized Kalman filter. Computationally, the decentralized Kalman filter can be viewed as a parallelization of the centralized Kalman filter, in which the central measurement process has been divided between the K distributed sensor nodes. Since (a) the local measureme measurement nt model at each node is smaller smaller (dimension M k for node k ) than the global measurement model (dimension M ), (b) the overhead associated with the combination stage (summations in ( 4.15 4.15)) )) is small, and (c) the nodes perform their computations in parallel, significant savings are afforded in terms of processing speed (ignoring communication delays). e decentralized Kalman filtering algorithm is summarized below. below.
4.2.1 EX 4.2.1 EXAMP AMPLE LE:: DIS DISTR TRIBUTE IBUTED D OBJ OBJECT ECT TR TRA ACK CKING ING We We consider the tracking of a moving object based on information information collected using multiple multiple sensor nodes. nodes. Such a task can arise during satellite and cellular cellular network-based network-based navigation, navigation, as depicted depicted in Figure 4.3 Figure 4.3.. We are interested in estimating the object’s position and velocity in a distributed manner, without making use of a central processor. Let .xn ; yn / denote the object’s position at time step n and .xP n ; Pyn / its velocity at time step n , so that the state vector x n D Œx n yn xP n yPn T is four-dimensional ( D D 4 ). Assume that
54
4. EXTE EXTENDE NDED D AND DEC DECENT ENTRAL RALIZE IZED D KAL KALMAN MAN FIL FILTER TERING ING
Algorithm 3 Decentralized Kalman filtering 3.1a), ), linear Gaussian measurement model (4.10 ( 4.10), ), Input : Linear Gaussian state evolution model ( 3.1a and an d a se sett of me meas asur urem emen ents ts fy1;k ; y2;k ; : : : ; yN;k g at ea each ch of K distribu distributed ted nod nodes es (k D 1 ) . 1;; 2 ; : : : ; K ). 1;; 2 ; : : : ; N , at each node k . Output : Estimates of the states x 1 ; x2 ; : : : ; xN at time steps n D 1 Initialize the mean and covariance of the Gaussian state distribution N .x0 I m0j0 ; P 0j0 / at time step n D 0 , at each node k . For time steps n D 1 1;; 2 ; : : : ; N , at each node k perform the following: 1. Carry out the predict prediction ion step mnjn1 P njn1
D D
Fn mn1jn1 ; Fn P n1jn1 FnT
CQ : n
1 1 2. Calculate Calculate the quantities quantities HT R H n;k and HT R y and communicate to all other n;k n;k n;k n;k n;k nodes. 3. Comp Compute ute the estimat estimatee Oxn D m njn of the state x n using the update step
P njn
mnjn
D D
K
P njn11
X C
1 HT n;k R n;k H n;k
k D1
!
1
;
K
P njn P njn11 mnjn1
X C
1 HT n;k R n;k y n;k
k D1
!
:
the object moves according to the approximately constant velocity model given in ( 3.12 3.12)) (see Subsection 3.3.1 Subsection 3.3.1). ). D 3 sensor nodes, with node 1 In this example, the measurement system is comprised of K D measur mea suring ing the object’ object’s x-posi x-positio tion, n, node node 2 mea measur suring ing the object’ object’s y-posi y-positio tion, n, and node node 3 mea measur suring ing the object’ object’s x-posi x-positio tion n and x-velo x-velocit cityy. us, the dimens dimension ionali ality ty of the local local mea measur sureme ements nts is M 1 D 1, M 2 D 1 , and M 3 D 2 , and the measurement vectors are (4.16a) Cw ; (4.16b) Cw ; (4.16c) Cw ; with the measurement matrices H D Œ1 0 0 0, H D Œ0 1 0 0, and H D , and the measurement noise covariance matrices R D , R D , and R D I . Note that yn;1 yn;2 yn;3
D D D
Hn;1 xn Hn;2 xn Hn;3 xn
n;1
n;1
n;1 n;2 n;3
n;2 2 y
n;3
n;2
2 y
the object’s y-velocity is not measured by any of the sensing nodes.
n;3
1 0 0 0 0 0 1 0
2 y
4.2.. DECENTR 4.2 DECENTRAL ALIZE IZED D KAL KALMAN MAN FIL FILTER TERING ING 55
position and Figure 4.3 Figure 4.3:: A satellite and cellular network-based navigation scenario. e automobile’s position velocity are estimated based on radio signals f rom navigation satellites within range and an d cellular net work data where available.
e decentralized Kalman filter is now used to estimate the moving object ’s position and velocity. velocity. e MATLAB MATLAB program distributed_tracking.m 4.3 performs performs distributed_tracking.m shown in Listing 4.3 N D D 100 time steps of distributed tracking using the decentralized Kalman filter. e state space model parameters were t D 0:5 , x D 0:1 , y D 0:25, and the initial object position and velocity were set as x0 D Œ1 1 0:1 0:1 T . Decentralized Kalman filtering is performed at each of the three nodes, and is initialized with Gaussian mean m 0j0 D 0 and covariance P 0j0 D I . For time steps n < 40 and n 70 , perfect communication occurs between all nodes. For time steps 4.4c shows shows plots of the 40 n < 70 , nodes 1 and 2 have no communication with node 3. Figure 4.4c actual and estimated object position and the mean square error (MSE) in position and velocity. e MSE is computed using Monte Carlo Car lo simulation of 1 million trajectories, with object states initialized as x 0 N .x0 I 0; I/. For comparison, the performance of the centralized Kalman filter (using the same data) implemented in the MATLAB function kalman_filter.m kalman_filter.m shown in Listing 3.1 Listing 3.1 is is also given. It can be seen that decentralized Kalman filtering is able to track the moving object accurately rately.. For For time time steps steps n < 40, the decent decentral ralize ized d Kalman Kalman filter filter estima estimates tes for all three three nodes nodes coinci coincide de with those from the centralized Kalman filter. filter. is is expected, as the filters have the same information, initialization, prediction, and update. For time steps 40 n < 70, the decentralized
56
4. EXTE EXTENDE NDED D AND DEC DECENT ENTRAL RALIZE IZED D KAL KALMAN MAN FIL FILTER TERING ING
Kalman Kalman filter estimates for nodes 1 and 2 are different different f rom those for node 3 (and also different from the centralized Kalman filter filter estimates) estimates).. In particular particular,, the decentralized decentralized estimates estimates have a higher error than the centralized estimates. is is because nodes 1 and 2 communicate their local information to compute the decentralized estimates, while node 3 uses its local information to compute the decentralized estimates (and the centralized Kalman filter still uses all information). For time steps n 70 , with normal communicat communications ions restored, restored, the decentralize decentralized d Kalman filter estimates for all three nodes converge back to those from the centralized Kalman filter.
% Distribute Distributed d Object Object Tracking using the Decentra Decentralized lized Kalman Kalman Filter Filter
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18
% State State space space model model delt = 0.5; % Time step interval F_n = eye(4) eye(4); ; F_n(1, F_n(1,3) 3) = delt;. delt;... .. F_n(2,4) = delt; % State-transition matrix sigma_ sigma_x x = 0.1; 0.1; Q_n = sigma_ sigma_x^2 x^2*[d *[delt elt^3/ ^3/3 3 0 delt^2 delt^2/2 /2 0;... 0;... 0 delt delt^3/ ^3/3 3 0 delt^2 delt^2/2; /2; delt^2 delt^2/2 /2 0 delt delt 0;... 0;... 0 delt elt^2/2 ^2/2 0 delt] elt]; ; % State tate noi noise cova covari ria ance nce matri atrix x sigma_y = 0.25; % Measurement noise standard deviation H_ H _n1 = [1 0 0 0]; % Node 1 measurement matrix R_n R_n1 = sigma_y^2; % Node 1 measurement noise covariance matrix H_ H _n2 = [0 1 0 0]; % Node 2 measurement matrix R_n R_n2 = sigma_y^2; % Node 2 measurement noise covariance matrix H_n3 = [1 0 0 0; 0; 0 0 1 0]; 0]; % Nod Node 3 mea measurement mat matrix R_n3 R_n3 = sigm sigma_ a_y^ y^2* 2*ey eye( e(2) 2); ; % Nod Node e 3 meas measur urem emen ent t nois noise e cova covari rian ance ce matr matrix ix H_n H_n = [H_ [H_n1 n1; ; H_n H_n2; 2; H_n3 H_n3]; ]; % Cen Centr tral al meas measur urem emen ent t mat matri rix x R_n = blkdiag(R blkdiag(R_n1,R _n1,R_n2,R _n2,R_n3); _n3);% % Central Central measurem measurement ent noise noise covarianc covariance e matrix matrix
19 20 21 22 23 24 25 26 27 28 29
% Initializa Initialization tion x(1:4, x(1:4,1) 1) = [1; 1; 0.1; 0.1; 0.1];% 0.1];% m1(1 m1(1:4 :4,1 ,1) ) = zero zeros( s(4, 4,1) 1); ; % P1(1 P1(1:4 :4,1 ,1:4 :4,1 ,1) ) = eye( eye(4) 4); ; % m2(1 m2(1:4 :4,1 ,1) ) = zero zeros( s(4, 4,1) 1); ; % P2(1 P2(1:4 :4,1 ,1:4 :4,1 ,1) ) = eye( eye(4) 4); ; % m3(1 m3(1:4 :4,1 ,1) ) = zero zeros( s(4, 4,1) 1); ; % P3(1 P3(1:4 :4,1 ,1:4 :4,1 ,1) ) = eye( eye(4) 4); ; % m(1: m(1:4, 4,1) 1) = zero zeros( s(4, 4,1) 1); ; % P(1: P(1:4, 4,1: 1:4, 4,1) 1) = eye eye(4 (4); ); %
Initial Initial Ini Initi tial al Init Initia ial l Ini Initi tial al Init Initia ial l Ini Initi tial al Init Initia ial l Init Initia ial l Ini Initi tial al
object object positi position on and velocit velocity y node node 1 Gau Gauss ssia ian n pos poste teri rior or mean mean node node 1 Gau Gauss ssia ian n post poster erio ior r cova covari rian ance ce node node 2 Gau Gauss ssia ian n pos poste teri rior or mean mean node node 2 Gau Gauss ssia ian n post poster erio ior r cova covari rian ance ce node node 3 Gau Gauss ssia ian n pos poste teri rior or mean mean node node 3 Gau Gauss ssia ian n post poster erio ior r cova covari rian ance ce cent centra ral l Gaus Gaussi sian an post poster erio ior r mean mean cent centra ral l Gau Gauss ssia ian n pos poste teri rior or cova covari rian ance ce
30 31
% Track Track object object using the decent decentral ralize ized d Kalman Kalman filter filter
4.2.. DECENTR 4.2 DECENTRAL ALIZE IZED D KAL KALMAN MAN FIL FILTER TERING ING 57 32
for n = 2 : 100,
% Time steps
33 34 35 36
% State State propagatio propagation n v_n v_n = mvnr mvnrnd nd([ ([0 0 0 0 0],Q 0],Q_n _n)' )'; ; % Stat State e nois noise e vect vector or x(1:4, x(1:4,n) n) = F_n*x(1 F_n*x(1:4, :4,n-1 n-1) ) + v_n; % Markov Markov linear linear Gauss Gaussian ian evolu evolutio tion n
37 38 39 40 41 42 43 44 45
% Generate Generate measuremen measurements ts w_n1 w_n1 = sigm sigma_ a_y* y*ra rand ndn( n(1, 1,1) 1); ; y_n1 y_n1 = H_n H_n1* 1*x( x(1: 1:4, 4,n) n) + w_n1 w_n1; ; w_n2 w_n2 = sigm sigma_ a_y* y*ra rand ndn( n(1, 1,1) 1); ; y_n2 y_n2 = H_n H_n2* 2*x( x(1: 1:4, 4,n) n) + w_n2 w_n2; ; w_n3 w_n3 = sigm sigma_ a_y* y*ra rand ndn( n(2, 2,1) 1); ; y_n3 y_n3 = H_n H_n3* 3*x( x(1: 1:4, 4,n) n) + w_n3 w_n3; ; y_n y_n = [y_n [y_n1 1; y_n y_n2; y_n3 y_n3]; ];
% % % % % % %
Node Node 1 meas measur urem emen ent t nois noise e vect vector or Node Node 1 line linear ar Gaus Gaussi sian an meas measur urem emen ents ts Node Node 2 meas measur urem emen ent t nois noise e vect vector or Node Node 2 line linear ar Gaus Gaussi sian an meas measur urem emen ents ts Node Node 3 meas measur urem emen ent t nois noise e vect vector or Node Node 3 line linear ar Gaus Gaussi sian an meas measur urem emen ents ts Cent entral ral linea inear r Gaus aussia sian mea measure surem ments ents
46 47
if (n<= (n<=40 40 || n>70 n>70) )
48 49
% Perfec Perfect t commun communica icatio tion n betwee between n all nodes nodes
50 51 52 53 54 55 56 57 58 59
% Decent Decentral ralize ized d Kalman Kalman filter filter comput computati ations ons at node node 1 % Predic Predict t m1_nn1 m1_nn1 = F_n*m1(1:4 F_n*m1(1:4,n-1) ,n-1); ; P1_nn1 P1_nn1 = F_n*P1(1:4 F_n*P1(1:4,1:4, ,1:4,n-1) n-1)*F_n *F_n' ' + Q_n; % Update Update P1(1:4,1: P1(1:4,1:4,n) 4,n) = inv(inv(P inv(inv(P1_nn1 1_nn1) ) + H_n1'*(R H_n1'*(R_n1\H _n1\H_n1) _n1) +... H_n2'*(R_n2\H_n2) H_n2'*(R_n2\H_n2) + H_n3'*(R_n3\H_n3)); H_n3'*(R_n3\H_n3)); m1(1:4,n) m1(1:4,n) = P1(1:4,1:4 P1(1:4,1:4,n)*( ,n)*(P1_nn P1_nn1\m1_ 1\m1_nn1 nn1 + H_n1'*... H_n1'*... (R_n1\y_n1 (R_n1\y_n1) ) + H_n2'*(R_ H_n2'*(R_n2\y_ n2\y_n2) n2) + H_n3'*(R_ H_n3'*(R_n3\y_ n3\y_n3)) n3)); ;
60 61 62 63 64 65 66 67 68 69 70
% Decent Decentral ralize ized d Kalman Kalman filter filter comput computati ations ons at node node 2 % Predic Predict t m2_nn1 m2_nn1 = F_n*m2(1:4 F_n*m2(1:4,n-1) ,n-1); ; P2_nn1 P2_nn1 = F_n*P2(1:4 F_n*P2(1:4,1:4, ,1:4,n-1) n-1)*F_n *F_n' ' + Q_n; % Update Update P2(1:4,1: P2(1:4,1:4,n) 4,n) = inv(inv(P inv(inv(P2_nn1 2_nn1) ) + H_n1'*(R H_n1'*(R_n1\H _n1\H_n1) _n1) +... H_n2'*(R_n2\H_n2) H_n2'*(R_n2\H_n2) + H_n3'*(R_n3\H_n3)); H_n3'*(R_n3\H_n3)); m2(1:4,n) m2(1:4,n) = P2(1:4,1:4 P2(1:4,1:4,n)*( ,n)*(P2_nn P2_nn1\m2_ 1\m2_nn1 nn1 + H_n1'*... H_n1'*... (R_n1\y_n1 (R_n1\y_n1) ) + H_n2'*(R_ H_n2'*(R_n2\y_ n2\y_n2) n2) + H_n3'*(R_ H_n3'*(R_n3\y_ n3\y_n3)) n3)); ;
58
4. EXTE EXTENDE NDED D AND DEC DECENT ENTRAL RALIZE IZED D KAL KALMAN MAN FIL FILTER TERING ING
% Decent Decentral ralize ized d Kalman Kalman filter filter comput computati ations ons at node node 3 % Predic Predict t m3_nn1 m3_nn1 = F_n*m3(1 F_n*m3(1:4,n:4,n-1); 1); P3_nn1 P3_nn1 = F_n*P3(1 F_n*P3(1:4,1: :4,1:4,n-1 4,n-1)*F_n )*F_n' ' + Q_n; % Update Update P3(1:4,1: P3(1:4,1:4,n) 4,n) = inv(inv(P inv(inv(P3_nn1 3_nn1) ) + H_n1'*(R_ H_n1'*(R_n1\H_ n1\H_n1) n1) +... H_n2'*(R_n2\H_n2) H_n2'*(R_n2\H_n2) + H_n3'*(R_n3\H_n3)); H_n3'*(R_n3\H_n3)); m3(1:4,n) m3(1:4,n) = P3(1:4,1:4 P3(1:4,1:4,n)*( ,n)*(P3_nn P3_nn1\m3 1\m3_nn1 _nn1 + H_n1'*... H_n1'*... (R_n1\y_ (R_n1\y_n1) n1) + H_n2'*(R_ H_n2'*(R_n2\y_ n2\y_n2) n2) + H_n3'*(R_n H_n3'*(R_n3\y_n 3\y_n3)); 3));
71 72 73 74 75 76 77 78 79 80 81
else
82
% Node Nodes s 1 and and 2 have have no comm commun unic icat atio ion n with with node node 3
83 84 85 86 87 88 89 90 91
92 93
% Decent Decentral ralize ized d Kalman Kalman filter filter comput computati ations ons at node node 1 % Predic Predict t m1_nn1 m1_nn1 = F_n*m1(1 F_n*m1(1:4,n:4,n-1); 1); P1_nn1 P1_nn1 = F_n*P1(1 F_n*P1(1:4,1: :4,1:4,n-1 4,n-1)*F_n )*F_n' ' + Q_n; % Update Update P1(1:4,1: P1(1:4,1:4,n) 4,n) = inv(inv(P inv(inv(P1_nn1 1_nn1) ) + H_n1'*(R_ H_n1'*(R_n1\H_ n1\H_n1) n1) +... H_n2'*(R_n2\H_n2)); m1(1:4,n) m1(1:4,n) = P1(1:4,1:4 P1(1:4,1:4,n)*( ,n)*(P1_nn P1_nn1\m1 1\m1_nn1 _nn1 + H_n1'*... H_n1'*... (R_n1\y_ (R_n1\y_n1) n1) + H_n2'*(R_ H_n2'*(R_n2\y_ n2\y_n2)); n2));
94 95 96 97 98 99 100 101 102 103
% Decent Decentral ralize ized d Kalman Kalman filter filter comput computati ations ons at node node 2 % Predic Predict t m2_nn1 m2_nn1 = F_n*m2(1 F_n*m2(1:4,n:4,n-1); 1); P2_nn1 P2_nn1 = F_n*P2(1 F_n*P2(1:4,1: :4,1:4,n-1 4,n-1)*F_n )*F_n' ' + Q_n; % Update Update P2(1:4,1: P2(1:4,1:4,n) 4,n) = inv(inv(P inv(inv(P2_nn1 2_nn1) ) + H_n1'*(R_ H_n1'*(R_n1\H_ n1\H_n1) n1) +... H_n2'*(R_n2\H_n2)); m2(1:4,n) m2(1:4,n) = P2(1:4,1:4 P2(1:4,1:4,n)*( ,n)*(P2_nn P2_nn1\m2 1\m2_nn1 _nn1 + H_n1'*... H_n1'*... (R_n1\y_ (R_n1\y_n1) n1) + H_n2'*(R_ H_n2'*(R_n2\y_ n2\y_n2)); n2));
104 105 106 107 108 109
% Decent Decentral ralize ized d Kalman Kalman filter filter comput computati ations ons at node node 3 % Predic Predict t m3_nn1 m3_nn1 = F_n*m3(1 F_n*m3(1:4,n:4,n-1); 1); P3_nn1 P3_nn1 = F_n*P3(1 F_n*P3(1:4,1: :4,1:4,n-1 4,n-1)*F_n )*F_n' ' + Q_n; % Update Update
4.2.. DECENTR 4.2 DECENTRAL ALIZE IZED D KAL KALMAN MAN FIL FILTER TERING ING 59
P3(1:4,1: P3(1:4,1:4,n) 4,n) = inv(inv(P inv(inv(P3_nn1 3_nn1) ) + H_n3'*(R H_n3'*(R_n3\H _n3\H_n3) _n3)); ); m3(1:4,n) m3(1:4,n) = P3(1:4,1:4 P3(1:4,1:4,n)*( ,n)*(P3_nn P3_nn1\m3_ 1\m3_nn1 nn1 + H_n3'*(R_n H_n3'*(R_n3\y_n 3\y_n3)); 3));
110 111 112 113
end
114 115 116 117 118
% Centraliz Centralized ed Kalman Kalman filter filter computat computations ions [m(1:4,n),P(1:4,1:4,n)] [m(1:4,n),P(1:4,1:4,n)] = kalman_filter (m(1:4,n-1),... (m(1:4,n-1),... P(1:4,1:4,n-1),y_n,F_n,Q_n,H_n,R_n); end
119 120 121 122 123 124 125 126 127 128
% Plot Plot actual actual and estima estimated ted object object positi position on figure, figure, plot(x(1, plot(x(1,:),x( :),x(2,:)) 2,:)); ; hold on, plot(m1(1, plot(m1(1,:),m1 :),m1(2,:) (2,:),'r') ,'r'); ; plot(m2(1,:),m2(2,:),'g--'); plot(m2(1,:),m2(2,:),'g--'); plot(m3(1,:),m3(2,:),'m-.' plot(m3(1,:),m3(2,:),'m-.'); ); plot(m(1,:),m(2,:),'k:'); xlabel('object x-position'); x-position'); ylabel('object ylabel('object y-position'); title('Dis title('Distribu tributed ted object object tracking tracking using the decentrali decentralized zed Kalman Kalman filter'); filter'); legend('actual','decentralized legend('actual','decentralized estimate estimate (node 1)',... 'decentral 'decentralized ized estimate estimate (node 2)','dece 2)','decentral ntralized ized estimate estimate (node (node 3)',... 3)',... 'centralized estimate');
Listing 4.3: distributed_tracking.m distributed_tracking.m
60
4. EXTE EXTENDE NDED D AND DEC DECENT ENTRAL RALIZE IZED D KAL KALMAN MAN FIL FILTER TERING ING
18
16
14
actual decentralized decentralized estimate (node 1) decentralized decentralized estimate (node 2) decentralized decentralized estimate (node 3) centralized centralized estimate
12
n o i t i s o10 p y t 8 c e j b o
6
4
2
0 0
1
2
3
4
5
6
7
8
9
object x-position (a) Object trajectory and tracking
Figure 4.4 Figure 4.4a: a: Distributed object tracking using the decentralized Kalman filter. e object moves according the dynamical law given in (3.12 ( 3.12), ), with t D 0:5 and x D 0:1 . Noisy measurements are collected using K D ( 4.16), ), with y D 0:25. N D D 3 sensor nodes as specified in (4.16 D 100 time steps of decentralized Kalman filtering are performed and the actual and estimated object position and the mean square error in position and velocity are shown in the plots. For time steps n < 40 and n 70 , perfect communication occurs between all nodes, and for time steps 40 n < 70, nodes 1 and 2 have no comm co mmun unic icat atio ion n wi with th no node de 3 (t (the hese se ti time me in inst stan ants ts ar aree in indi dica cate ted d on th thee ob obje ject’ ct’s tra traje ject ctory ory wi with th cy cyan an ‘ ’s). For comparison, the performance of a centralized Kalman filter (using the same data) is also given.
4.2.. DECENTR 4.2 DECENTRAL ALIZE IZED D KAL KALMAN MAN FIL FILTER TERING ING 61
18
actual decentralized decentralized estimate (node 1) decentralized decentralized estimate (node 2) decentralized decentralized estimate (node 3) centralized estimate
17
16
15
n o i t i s o14 p y t 13 c e j b o
12
11
10
9 6
6.5
7
7.5
8
8.5
9
object x-position (b) Zoom of the region region 6 x 9 and 9 y 18
Figure 4.4b 4.4b:: Distributed object tracking using the decentralized Kalman filter. e object moves according the dynamical law given in (3.12 ( 3.12), ), with t D 0:5 and x D 0:1 . Noisy measurements are collected using K D ( 4.16), ), with y D 0:25. N D D 3 sensor nodes as specified in (4.16 D 100 time steps of decentralized Kalman filtering are performed and the actual and estimated object position and the mean square error in position and velocity are shown in the plots. For time steps n < 40 and n 70 , perfect communication occurs between all nodes, and for time steps 40 n < 70, nodes 1 and 2 have no comm co mmun unic icat atio ion n wi with th no node de 3 (t (the hese se ti time me in inst stan ants ts ar aree in indic dicat ated ed on th thee ob obje ject’ ct’s tr traje aject ctory ory wi with th cy cyan an ‘ ’s). For comparison, the performance of a centralized Kalman filter (using the same data) is also given.
62
4. EXTE EXTENDE NDED D AND DEC DECENT ENTRAL RALIZE IZED D KAL KALMAN MAN FIL FILTER TERING ING
decentralized estimate (node 1) decentralized estimate (node 2) decentralized estimate (node 3) centralized estimate
2
10
r o r r 101 e e r a u q s n a 0 e 10 M
−1
10
0
20
40
60
80
100
time step (c) Mean square error (MSE) in estimation of object position and velocity
Figure 4.4 Figure 4.4c: c: Distributed object tracking using the decentralized Kalman filter. e object moves according the dynamical law given in (3.12 ( 3.12), ), with t D 0:5 and x D 0:1 . Noisy measurements are collected using K D ( 4.16), ), with y D 0:25. N D D 3 sensor nodes as specified in (4.16 D 100 time steps of decentralized Kalman filtering are performed and the actual and estimated object position and the mean square error in position and velocity are shown in the plots. For time steps n < 40 and n 70 , perfect communication occurs between all nodes, and for time steps 40 n < 70, nodes 1 and 2 have no comm co mmun unic icat atio ion n wi with th no node de 3 (t (the hese se ti time me in inst stan ants ts ar aree in indi dica cate ted d on th thee ob obje ject’ ct’s tra traje ject ctory ory wi with th cy cyan an ‘ ’s). For comparison, the performance of a centralized Kalman filter (using the same data) is also given.
63
CHAPTER 5
Conclusion In this manuscript, the Kalman filter was introduced as a tool for statistical estimation in the context of linear Gaussian dynamical systems. Several example applications of the Kalman filter were explored, along with extensions and implementation in MATLAB. Estimation techniques were first introduced in Chapter 2 Chapter 2.. Methods and example applications were presented for maximumlikelihoo likelihood, d, Bayesian, Bayesian, and sequential sequential Bayesian Bayesian estimators estimators.. e Kalman Kalman filter was derived derived as the sequential sequential Bayesian Bayesian solution to an example example problem of estimating estimating the state of a dynamical dynamical system evolving according to a simple 1-D linear Gaussian state space model. In Chapter 3 3,, the Kalman filter was presented formally as an estimation algorithm for the general multivariate linear Gaussian dynamical dynamical system. system. Several examples examples of Kalman filtering filtering were given, including its relation to the RLS adaptive filter. Extensions of the Kalman filter were discussed in Chapter 4 4.. e extended Kalman filter was outlined as a solution to the nonlinear nonlinear Gaussian estimation problem. A framework for distributed estimation using Kalman filtering was also presented. In each case, case, exampl examples es and app applic licati ations ons wer weree pro provid vided. ed. e exampl examples es wer weree supple supplemen mented ted with with MATLA MATLAB B implementation, and graphs generated from these programs to better illustrate the concepts. is manuscript serves as an introduction to Kalman filtering and provides a glimpse into some of its features, applications, and extensions. For further coverage interested readers are referred to the literature: for general Kalman filtering [ 6, 12 12,, 35 35,, 36 36], ], Kalman filtering and MATLAB [37 [37], ], Kalman filtering implementation and applications [ 38 38––40 40], ], and Kalman filter extensions [5 [5, 41 41]. ].
65
Notation parameters of interest; state vector dimension of state vector observed data; measurement vector dimension of measurement vector probability ty density function function (pdf) (pdf ) probabili j conditional pdf O estimate of state x number of measurements; number of time steps expected value identity matrix vector or matrix transpose discrete time cross-correlation cross-correlation of random processes x n and y n for lag i state vector at time step n measurement vector at time step n set of measurements up to time step n O estimate of state vector at time step n state-transition matrix at time step n state noise vector at time step n state noise covariance matrix at time step n measurement matrix at time step n measurement noise vector at time step n measurement noise covariance matrix at time step n N Gaussian distribution m mean standard deviation posterior state distribution at time step n computed using mnjn mean of Gaussian posterior measurements up to time step n covariance of Gaussian posterior state distribution at time step n computed P njn using measurements up to time step n state-transition function f ./ measurement function h./ Jacobian of state-transition function evaluated at m n1jn1 FQ n Q n Jacobian of measurement function evaluated at mnjn1 H
x D y M p. / p. / x N EŒ I Œ:::T n xy ri xn yn Y n xn Fn vn Qn Hn wn Rn
66
NOTA NOT ATIO TION N
K yn;k M k Hn;k wn;k Rn;k
number of sensor nodes measurement vector at time step n for node k dimension of measurement vector for node k measurement matrix at time time step n for node k measurement noise vector at time step n for node k measurement noise covariance matrix matrix at time step n for node k
67
Bibliography [1] H. L. V. Trees, Detection, Estimation, and Modulation eory, Part I . 2001. DOI: 2001. DOI: 10.1002/047122108 10.1002/04712210822 .
Wiley Wiley Inters Interscie cience nce,,
[2] A. Papoulis and S. U. Pillai, Probability, Random Variables and Stochastic Processes , 4th ed. McGraw-Hill, 2002. [3] R. Kalman, “A new approach to linear filtering and prediction problems,” problems,” Trans Trans.. ASME ASME Ser. Ser. 10.1115/1.3662552 . D. J. Basic Eng. , vol. 82, pp. 35–45, 1960. DOI: 10.1115/1.3662552. [4] R. Kalman Kalman and R. Bucy, Bucy, “New “New resul results ts in linear linear filteri filtering ng and pre predic dictio tion n theory theory,” ,” Trans. Trans. ASME 10.1115/1.3658902 . Ser. D. J. Basic Eng. , vol. 83, pp. 95–108, 1961. DOI: 10.1115/1.3658902. [5] B. Ristic, S. Arulampalam, and N. Gordon, Beyond the Kalman Filter: Particle Filters for Artech House, House, 2004. 2004. Tracking Applications . Artech [6] Y. Bar-Shalom, X. R. Li, and T. Kirubarajan, Estimation with Applications to Tracking and York: John Wiley & Sons, 2001. DOI: 2001. DOI: 10.1002/0471221279. 10.1002/0471221279 . Navigation Navigation, 1st ed. New York: [7] B. Cipra, “Engineers look to Kalman filtering for guidance,” SIAM News , vol. 26, 1993. [8] M. S. Grewal, L. R. Weill, and A. P. P. Andrews, Global Positioning Systems, Inertial NavigaWiley-Interscience ience,, 2001. 2001. tion, and Integration Integration . Wiley-Intersc [9] D. Titterton and J. Weston, Strapdown Inertial Navigation Technology , 2nd 2nd ed. 2004. DOI: 2004. DOI: 10.1049/PBRA017E. 10.1049/PBRA017E .
AIA AIAA,
[10] R. O. Duda, Duda, P. E. Ha Hart rt,, and and D.G. Stor Stork, k, Pattern 2nd ed. ed. Wiley Wiley Inte Inters rsci cien ence ce,, Pattern Classification, 2nd 2001. [11] D. J. C. MacKay, Information Cambridge dge UniUni Information eory, eory, Inference, Inference, and Learning Learning Algorithms Algorithms . Cambri versity Press, 2003. [12] S. Haykin, Adaptive Filter eory , 4th ed. Prentic Prenticee Hall, 2001. [13] A. Gelman, J. B. Carlin, H. S. Stern, and D. B. Rubin, Bayesian Data Analysis , 2nd ed. CRC Press, 2004. [14] T. T. Baye Bayess and and R. Pric Price, e, “An essa essayy towa toward rdss solvi solving ng a prob proble lem m in the the doct doctri rine ne of chan chance ces. s. By the the late late rev rev. Mr Mr.. Baye Bayes, s, F. R. S. Comm Commun unic icat ated ed by Mr Mr.. Pric Price, e, in a lett letter er to John John Canto Canton, n, A. M. F. R. S.” Philosophical 10.1098/rstl.1763.0053 053.. Philosophical Transactions Transactions , vol. 53, pp. 370–418, 1763. DOI: 10.1098/rstl.1763.0
68
BIBLI BIB LIOGR OGRAPHY APHY
[15] W. R. Gilks, S. Richardson, and D. J. Spiegelhalter, Eds., Markov Chain Monte Carlo in Chapman & Hall/CRC, Hall/CRC, 1996. 1996. DOI: 10.1007/978-1-489 10.1007/978-1-4899-4485-6 9-4485-6.. Practice . Chapman [16] A. Doucet, N. D. Freitas, and N. Gordon, Eds., Sequential Monte Carlo methods in practice , 1st ed. Springer Springer,, 2001. 2001. DOI: 10.1007/978-1-475 10.1007/978-1-4757-3437-9 7-3437-9.. [17] G. H. Golub and C. F. F. V. V. Loan, Matrix Computations , 3rd 3rd ed. Baltimore Baltimore:: Johns Johns Hopkins Hopkins University Press, 1996. [18] S. Edla, J. Zhang, J. Spanias, N. Kovvali, A. Papandreou-Suppappola, and C. Chakrabarti, “Adaptive parameter estimation of cardiovascular signals using sequential bayesian techniques, niques,”” in Signals, Signals, Systems and Computers Computers (ASILOMAR), (ASILOMAR), 2010 Conference Conference Record of the Forty Fourth Asilomar Conference on , nov. 2010, pp. 374 –378. DOI: 10.1109/ACSSC.2010.5757538.. SSC.2010.5757538 [19] K. Tu, H. ornbu ornburg rg,, M. Fulme Fulmerr, and A. Spa Spania nias, s, “Trac “Trackin kingg the path path shape shape qua quallitie itiess of huma human n moti motion on,” ,” in Acousti Acoustics, cs, Speec Speechh and Signal Signal Process Processing ing,, 2007. 2007. ICASSP ICASSP 2007. IEEE International Conference on, vol. 2, april 2007, pp. II–781 –II–784. DOI: 10.1109/ICASSP.2007.366352 . [20] R. E. Lawrence and H. Kaufman, “e Kalman filter for the equalization of a digital communications channel,” IEEE Transactions on Communication Technology , vol. COM-19, pp. 1137–1141, 1971. DOI: 1971. DOI: 10.1109/TC 10.1109/TCOM.1971.1090786 OM.1971.1090786.. [21] A. B. Nara Narasi simh mham amurt urthy hy,, M. K. Bana Banava varr, and and C. Teped epedel elen enli liog oglu lu,, OFDM OFDM SysSysMorg Morgaan & Cla Clayp ypoo ooll Pub Publi lish sheers, rs, 201 20100. DOI: tems for Wireless Wireless Communica Communications tions . 10.2200/S00255ED1V01Y201002ASE005.. 10.2200/S00255ED1V01Y201002ASE005 [22] N. Nair and A. Spanias, “Fast adaptive algorithms using eigenspace projections,” projections,” in Signals, System Systemss and Comput Computers, ers, 1994. 1994. 1994 1994 Confere Conference nce Record Record of the Twentywenty-Eig Eighth hth Asilom Asilomar ar ConferConfer10.1109/ACSSC.1994.471712 CSSC.1994.471712.. ence ence on, vol. 2, oct-2 nov 1994, pp. 1520 –1524 vol.2. DOI: 10.1109/A [23] N. Nair and A. Spanias, “Gradient eigenspace projections for adaptive filtering,” in Circuits and Systems, 1995., Proceedings., Proceedings of the 38th Midwest Symposium on , vol. 1, aug 1995, pp. 259 –263 vol.1. DOI: 10.1109/MWSC 10.1109/MWSCAS.1995. AS.1995.504427 504427.. [24] C. Panayiotou, A. Spanias, and K. Tsakalis, “Channel equalization using the g-probe,” in Circuits and Systems, 2004. ISCAS ’04. Proceedings of the 2004 International Symposium on , vol. 3, may 2004, pp. III II I – 501–4 Vol.3. Vol.3. DOI: 10.1109/ISCA 10.1109/ISCAS.2004.13 S.2004.1328793 28793.. [25] J. Foutz and A. Spanias, “An “An adaptive low rank algorithm for semispherical antenna arrays,” in Circuits and Systems, 2007. ISCAS 2007. IEEE International Symposium on , may 2007, pp. 113 –116. D –116. DOI: OI: 10.1109/ISC 10.1109/ISCAS.2007. AS.2007.378234 378234..
BIBLI BIB LIOGR OGRAPHY APHY 69
[26] T. Gupta, S. Suppappola, and A. Spanias, “Nonlinear acoustic echo control using an accelerometer,” celerometer,” in Acoustics, Speech and Signal Processing, 2009. ICASSP 2009. IEEE International Conference on , april 2009, pp. 1313 –1316. DOI: 10.1109/ICASSP.2009.4959833 . [27] MATLAB documentation for the RLS adaptive filter function adaptfilt.rls , available online at http://www.mat http://www.mathworks.com/help/ hworks.com/help/toolbox/dsp/ref toolbox/dsp/ref/adaptfilt.rls. /adaptfilt.rls. html. [28] R. OlfatiOlfati-Sab Saber er,, “Distri “Distribute buted d Kalman Kalman filteri filtering ng for sensor sensor network networks,” s,” in 46th IEEE Orleans, LA, 2007, pp. pp. 5492–5 5492–5498 498.. DOI: Conference on Decision and Control , New Orleans, 10.1109/CDC.2007.4434303.. 10.1109/CDC.2007.4434303 [29] C. Y. Chong, S. Mori, and K. C. Chang, “Distributed multitarget multisensor tracking,” in Multitarget-Multisensor Tracking: Advanced Applications , Y. Bar-Sh Bar-Shalo alom, m, Ed. Artech Artech House, 1990, ch. 8, pp. 247–295. [30] K. C. Chang, C. Y. Chong, and Y. Bar-Shalom, “Distributed estimation in distributed sensor networks,” in Large-Scale Stochastic Systems Detection, Estimation, Stability and Control , S. G. Tzafestas and K. Watanabe, Watanabe, Eds. Marcel Dekker, Dekker, 1992, ch. ch. 2, pp. pp. 23–71. [31] S. J. Julier and J. K. Uhlmann, “A “A new extension of the Kalman filter to nonlinear systems,” in Proceed Proceeding ingss of AeroSe AeroSense nse:: e 11th 11th Sympos Symposium ium on Aerosp Aerospace ace/Def /Defenc encee Sensin Sensing, g, Simula Simulatio tion n and Controls , Orlando, FL, 1997. [32] A. A. Berryman, “e orgins and evolution of predator-prey theory,” Ecology , vol. 73, pp. 1530–1535, 1992. DOI: 1992. DOI: 10.2307/1940005. 10.2307/1940005 . [33] C. Moler, “Experiments with MATLAB,” http://www.math http://www.mathworks.com/moler works.com/moler/exm/ /exm/ index.html, 2011. [34] B. S. Y. Rao, H. F. Durrant-Whyte, and J. A. Sheen, “A fully decentralized multi-sensor system for tracking and surveillance,” e International Journal of Robotics Research , vol. 12, pp. 20–44, 1993. DOI: 1993. DOI: 10.1177/0278364993 10.1177/027836499301200102 01200102.. [35] T. Kailath, A. H. Sayed, and B. Hassibi, Linear Estimation . Prent Prentice ice Hal Hall, l, 2000. 2000. [36] D. Simon, Optimal State Estimation: Kalman, H Infty, and Nonlinear Approaches . Wiley and Sons, 2006. DOI: 2006. DOI: 10.1002/0470045345. 10.1002/0470045345 .
John
[37] M. S. Grewal and A. P. Andrews, Kalman Filtering: eory and Practice Using MATLAB . John Wiley and Sons, 2008. DOI: 2008. DOI: 10.1002/97804703 10.1002/9780470377819 77819.. [38] R. L. Eubank, A Kalman Filter Primer . CRC Press Press,, 2006. 2006. [39] C. K. Chui and G. Chen, Kalman Springer, 2009. Kalman Filtering: Filtering: with Real-Time Real-Time Applicatio Applications ns . Springer
70
BIBLI BIB LIOGR OGRAPHY APHY
[40] P. Zarchan and H. Musoff, Fundamentals of Kalman Filtering: A P ractical Approach , 3rd ed. American Institute of Aeronautics and Astronautics, Inc., 2009. [41] S. Haykin, Ed., Kal 2001. DOI: Kalman man Filte Filtering ring and Neur Neural al Networ Networks ks . John Wiley and Sons, 2001. DOI: 10.1002/0471221546.. 10.1002/0471221546
71
Authors’ Biog Biographies raphies NARAYAN NARAY AN KOVV KOVVALI ALI Narayan Narayan Kovvali Kovvali received the B.Tech. degree in electrical engineering from the Indian Institute of Technology Technology,, Kharagpur, India, in 2000, and the M.S. and Ph.D. degrees in electrical engineering from Duke University, Durham, North Carolina, in 2002 and 2005, respectively. In 2006, he joined the Department of Electrical Engineering at Arizona State University, University, Tempe, Arizona, as Assistant Research Scientist. He currently holds the position of Assistant Research Professor in the School of Electrical, Computer, and Energy Engineering at Arizona State University. University. His research interests include statistical signal processing, detection, estimation, stochastic filtering and tracking, Bayesian data analysis, multi-sensor data fusion, Monte Carlo methods, and scientific computing. Dr. Kovvali is a Senior Member of the IEEE.
MAHESH BANA BANAV VAR is a post-doctoral researcher in the School of Electrical, Computer and Energy Mahesh Mahesh Banavar Banavar is Engineering at Arizona State University. He received the B.E. degree in Telecommunications Engine Engineeri ering ng from Visvesv Visvesvara araya ya Technol echnologi ogical cal Univer Universit sityy, Karnata Karnataka, ka, India, India, in 2005, 2005, and the M.S M.S.. and Ph.D. degrees in Electrical Engineering from Arizona State University in 2007 and 2010, respectively. His research area is Signal Processing and Communications, and he is specifically working on Wireless Communications and Sensor Networks. He is a member of MENSA and the Eta Kappa Nu honor society.
ANDREAS ANDREA S SP SPANIAS ANIAS Andreas Spanias is Spanias is Professor in the School of Electrical, Computer, and Energy Engineering at Arizona State University University (ASU). He is also the founder and director director of the SenSIP Industry Consortium. His research interests are in the areas of adaptive signal processing, speech processing, and audio sensing. He and his student team developed the computer simulation software Java-DSP ( J-DSP–ISBN 0-9724984-0-0). He is author of two text books: Audio Processing and by Wiley and DSP; An Interactive Approach. He served as Associate Editor of the IEEE Coding by and as General Co-chair of IEEE ICASSP-99. He also served Transactions Transactions on Signal Processing and as the IEEE Signal Processing Vice-President for Conferences. Andreas Spanias is co-recipient of the 2002 IEEE Donald G. Fink paper prize award and was elected Fellow of the IEEE in 2003. He served as Distinguished Lecturer for the IEEE Signal Processing Society in 2004.
