[PPT] - Kalman Filter State Space Model Review Derivation x n +1 = F n x n PowerPoint Presentation

SLIDE 1

Key Linear Estimation Properties Linearity of Linear MMSE Estimators

(A1x1 + A2x2) = A1 ˆ

x1 + A2 ˆ x2 for any two matrices A1 and A2 The Orthogonality Condition ˆ x|y1,y2 = ˆ x|y1 + ˆ x|y2 if and only if y1⊥y2 (Ry1y2 = 0)

J. McNames

Portland State University ECE 539/639 Kalman Filter

Ver. 1.03

3

Kalman Filter

Derivation
Examples
Time and Measurement Updates
J. McNames

Portland State University ECE 539/639 Kalman Filter

Ver. 1.03

1

State Covariance Recursion Recall that Πn+1 xn+1, xn+1 = Fnxn + Gnun, Fnxn + Gnun = Fnxn, xnF ∗

n + Gnun, unG∗ n

= FnΠnF ∗

n + GnQnG∗ n

where the initial value Π0 is usually specified by the user.

J. McNames

Portland State University ECE 539/639 Kalman Filter

Ver. 1.03

4

State Space Model Review xn+1 = Fnxn + Gnun yn = Hnxn + vn ⎡ ⎣ x0 un vn ⎤ ⎦ , ⎡ ⎢ ⎢ ⎣ x0 uk vk 1 ⎤ ⎥ ⎥ ⎦

=

⎡ ⎣ Π0 Qnδnk Snδnk S∗

nδnk

Rnδnk ⎤ ⎦ Fn ∈ Cℓ×ℓ Gn ∈ Cℓ×m Hn ∈ Cp×ℓ xn ∈ Cℓ×1 un ∈ Cm×1 yn ∈ Cp×1 vn ∈ Cp×1 un, xk = 0 vn, xk = 0 for n ≥ k un, yk = 0 vn, yk = 0 for n > k un, yk = Sn vn, yk = Rn for n = k un, x0 = 0 iff un, xn = 0 for n ≥ 0

J. McNames

Portland State University ECE 539/639 Kalman Filter

Ver. 1.03

2

SLIDE 2

State Estimation for Innovations ˆ xn+1|n = xn+1, col{y0, . . . , yn} col{y0, . . . , yn}−2 col{y0, . . . , yn} = xn+1, col{e0, . . . , en} col{e0, . . . , en}−2 col{e0, . . . , en} =

n

k=0

xn+1, ekR−1

e,kek

where we have defined Re,k ek2 = E [eke∗

k]

The first equality is just the solution to the normal equations
The second equality follows from the definition of the innovations

and equivalence of the linear spaces spanned by the observations and innovations L{y0, . . . , yn} = L{e0, . . . , en}

The third equality follows from the orthogonality property
J. McNames

Portland State University ECE 539/639 Kalman Filter

Ver. 1.03

7

Innovations Let us begin by just using the linear process model and the definition

f the innovations

yn = Hnxn + vn en yn − ˆ yn|n−1

The estimator notation used here is

ˆ yn|k = the linear MMSE estimator of yn given {y0, . . . , yk}

Our goal is to come up with a recursive formulation for linear

estimators of xn for n = 0, 1, . . .

Our motivation for using the innovations is that they have a

diagonal covariance matrix

Recall that calculating the innovations is equivalent to applying a

whitening filter – This has become a theme for this class – “Whiten before processing” – Simplifies the subsequent step of estimation

J. McNames

Portland State University ECE 539/639 Kalman Filter

Ver. 1.03

5

State Estimation for Innovations ˆ xn+1|n =

n

k=0

xn+1, ekR−1

e,kek

This expression assumes the error covariance matrix is invertible

(positive definite) Re,k > 0

This is a nondegeneracy assumption on the process

{y0, . . . , yn}

It essentially means that no variable yn can be exactly estimated

by a linear combination of earlier observations

This does not require that Rn = vn2 > 0

– It’s quite possible that Re,k > 0 even if Rn = 0

However, if Rn > 0, we know for certain that Re,k > 0
J. McNames

Portland State University ECE 539/639 Kalman Filter

Ver. 1.03

8

Recursion for Innovations en yn − ˆ yn|n−1 yn = Hnxn + vn ˆ yn|n−1 = Hn ˆ xn|n−1 + ˆ vn|n−1 = Hn ˆ xn|n−1 en = yn − Hn ˆ xn|n−1 = Hnxn + vn − Hn ˆ xn|n−1 = Hn(xn − ˆ xn|n−1) + vn = Hn ˜ xn|n−1 + vn where the new notation is defined as ˜ xn|n−1 xn − ˆ xn|n−1 ˜ yn|n−1 yn − ˆ yn|n−1 = en Note that our goal of finding an expression for the innovations reduces to finding a way to estimate the one-step predictions of the state vector, ˆ xn|n−1

J. McNames

Portland State University ECE 539/639 Kalman Filter

Ver. 1.03

6

SLIDE 3

State Prediction xn+1 = Fnxn + Gnun ˆ xn+1|n−1 = Fn ˆ xn|n−1 + Gn ˆ un|n−1 = Fn ˆ xn|n−1 Now given ˆ xn+1|n = ˆ xn+1|n−1 + xn+1, enR−1

e,n

yn − Hn ˆ

xn|n−1

we can set up a proper recursion that we initialize with e0 = y0

en = yn − Hn ˆ xn|n−1 ˆ xn+1|n = Fn ˆ xn|n−1 + Kp,nen where we have defined Kp,n xn+1, enR−1

e,n

The subscript p indicates that this gain is used to update a

predicted estimator of the state, ˆ xn+1|n

This is one of the Kalman gains that we will use
J. McNames

Portland State University ECE 539/639 Kalman Filter

Ver. 1.03

11

Circular Reasoning? In order to calculate the innovations en yn − ˆ yn|n−1 = yn − Hn ˆ xn|n−1 we need to estimate the one step predictions of the state. Yet in order to obtain these estimates, ˆ xn+1|n =

n

k=0

xn+1, ekR−1

e,kek

we need the innovations!

Luckily, we only need the previous innovations to estimate the one

step state predictions

Suggests a recursive solution
J. McNames

Portland State University ECE 539/639 Kalman Filter

Ver. 1.03

9

Defining the State Error Covariance ˜ xn|n−1 xn − ˆ xn|n−1 Pn|n−1 ˜ xn|n−12 = E

˜

xn|n−1 ˜ x∗

n|n−1

We still need to solve for Kp,n and Re,n in terms of the known

state space model parameters

To do so it will be useful to introduce yet another covariance

matrix

Pn|n−1 is the state error covariance matrix
It would be great to know this anyway

– Gives us a means of knowing how accurate our estimates are – If {x0, u0, . . . , un, v0, . . . , vn} are jointly Gaussian (central limit theorem) we could construct exact confidence intervals on

ur estimates!
J. McNames

Portland State University ECE 539/639 Kalman Filter

Ver. 1.03

12

Recursive State Estimation ˆ xn+1|n =

n

k=0

xn+1, ekR−1

e,kek

= n−1

k=0

xn+1, ekR−1

e,kek

+ xn+1, enR−1

e,nen

= ˆ xn+1|n−1 + xn+1, enR−1

e,n

yn − Hn ˆ

xn|n−1

This is almost what we need
However, there are still some missing pieces

– Can we express ˆ xn+1|n−1 in terms of what is known at time n: ˆ xn|n−1 and en? – How do we obtain an expression for the error covariance matrix in terms of the model parameters?

The only way to make any further progress is to use the state

update equation that relates xn+1 to xn

J. McNames

Portland State University ECE 539/639 Kalman Filter

Ver. 1.03

10

SLIDE 4

Solving for the Kalman Prediction Gain Continued Kp,n xn+1, enR−1

e,n

xn+1, en = Fnxn, en + Gnun, en xn, en = Pn|n−1H∗

n

We still need to solve for the second term un, en = un, Hn ˜ xn|n−1 + vn = un, ˜ xn|n−1H∗

n + un, vn

= un, xn − ˆ xn|n−1H∗

n + un, vn

= un, vn = Sn Thus, Kp,n xn+1, enR−1

e,n =

FnPn|n−1H∗

n + GnSn

R−1

e,n

The only thing we still need is an expression for is Pn|n−1

J. McNames

Portland State University ECE 539/639 Kalman Filter

Ver. 1.03

15

Solving for the Innovations Covariance en = yn − Hn ˆ xn|n−1 = Hnxn + vn − Hn ˆ xn|n−1 = Hn(xn − ˆ xn|n−1) + vn = Hn ˜ xn|n−1 + vn Then it follows immediately that Re,n = en, en = Hn ˜ xn|n−1 + vn, Hn ˜ xn|n−1 + vn = Hn˜ xn|n−1, ˜ xn|n−1H∗

n + vn, vn

= HnPn|n−1H∗

n + Rn

Thus we have replaced the problem of needing an expression for Re,n with the problem of needing an expression for Pn|n−1

J. McNames

Portland State University ECE 539/639 Kalman Filter

Ver. 1.03

13

Solving for the State Error Covariance Let us find a recursive expression for the residuals ˜ xn|n−1 that we can then use to solve for Pn|n−1 ˜ xn+1|n = xn+1 − ˆ xn+1|n = (Fnxn + Gnun) −

Fn ˆ

xn|n−1 + Kp,nen

= Fn
xn − ˆ

xn|n−1

+ Gnun − Kp,n
Hn ˜

xn|n−1 + vn

= (Fn − Kp,nHn)˜

xn|n−1 + Gn −Kp,n un vn

= Fp,n ˜

xn|n−1 + Gn −Kp,n un vn

Then

Pn+1|n ˜ xn+1|n, ˜ xn+1|n = Fp,nPn|n−1F ∗

p,n +

Gn −Kp,n Qn Sn S∗

n

Rn G∗

n

−K∗

p,n

J. McNames

Portland State University ECE 539/639 Kalman Filter

Ver. 1.03

16

Solving for the Kalman Prediction Gain Kp,n xn+1, enR−1

e,n

We have an expression for Re,n, but we still need an expression for the cross-covariance in terms of the state space model parameters xn+1, en = Fnxn + Gnun, en = Fnxn, en + Gnun, en xn, en = xn, Hn ˜ xn|n−1 + vn = xn, ˜ xn|n−1H∗

n + xn, vn

= xn, ˜ xn|n−1H∗

n

= ˆ xn|n−1 + ˜ xn|n−1, ˜ xn|n−1H∗

n

= ˆ xn|n−1, ˜ xn|n−1H∗

n + ˜

xn|n−1, ˜ xn|n−1H∗

n

= ˜ xn|n−1, ˜ xn|n−1H∗

n

= Pn|n−1H∗

n

J. McNames

Portland State University ECE 539/639 Kalman Filter

Ver. 1.03

14

SLIDE 5

Covariance form of the Kalman Filter User provides the state space model parameters. Initialization ˆ x0|−1 = 0 P0|−1 = Π0 Recursions en = yn − Hn ˆ xn|n−1 Re,n = HnPn|n−1H∗

n + Rn

Kp,n =

FnPn|n−1H∗

n + GnSn

R−1

e,n

ˆ xn+1|n = Fn ˆ xn|n−1 + Kp,nen Pn+1|n = FnPn|n−1F ∗

n + GnQnG∗ n − Kp,nRe,nK∗ p,n

= FnPn|n−1F ∗

n + GnQnG∗ n − Kp,n(HnPn|n−1F ∗ n + S∗ nG∗ n)

J. McNames

Portland State University ECE 539/639 Kalman Filter

Ver. 1.03

19

Solving for the State Error Covariance Continued Pn+1|n

˜

xn+1|n, ˜ xn+1|n = Fp,nPn|n−1F ∗

p,n +

Gn −Kp,n Qn Sn S∗

n

Rn G∗

n

−K∗

p,n

=

Fp,nPn|n−1F ∗

p,n +

Gn

−Kp,n QnG∗

n − SnK∗ p,n

S∗

nG∗ n − RnK∗ p,n

=

(Fn − Kp,nHn)Pn|n−1(Fn − Kp,nHn)∗ +GnQnG∗

n − GnSnK∗ p,n − Kp,nS∗ nG∗ n + Kp,nRnK∗ p,n

= FnPn|n−1F ∗

n − Kp,nHnPn|n−1F ∗ n

−FnPn|n−1H∗

nK∗ p,n + Kp,nHnPn|n−1H∗ nK∗ p,n

+GnQnG∗

n − GnSnK∗ p,n − Kp,nS∗ nG∗ n + Kp,nRnK∗ p,n

J. McNames

Portland State University ECE 539/639 Kalman Filter

Ver. 1.03

17

Kalman Filter Features

Requires only O(ℓ3) operations per update if m ≪ ℓ and p ≪ ℓ
To obtain ˆ

xN|N−1 requires a total of O(Nℓ3) operations— two

rders of magnitude less than solving the normal equations

directly!

Gives the error covariance matrices

– Easily obtain standard deviation of the error – Approximate confidence intervals

Estimates the state at all times {0, . . . , N}
Includes a whitening filter as part of the computations
J. McNames

Portland State University ECE 539/639 Kalman Filter

Ver. 1.03

20

Solving for the State Error Covariance Continued Pn+1|n = FnPn|n−1F ∗

n + GnQnG∗ n + Kp,n

HnPn|n−1H∗

n + Rn

K∗

p,n

−Kp,n

HnPn|n−1F ∗

n + S∗ nG∗ n

−
FnPn|n−1H∗

n + GnSn

K∗

p,n

Re,n = HnPn|n−1H∗

n + Rn

Kp,n =

FnPn|n−1H∗

n + GnSn

R−1

e,n

Pn+1|n = FnPn|n−1F ∗

n + GnQnG∗ n + Kp,nRe,nK∗ p,n

−Kp,nRe,nK∗

p,n

−Kp,nRe,nK∗

p,n

= FnPn|n−1F ∗

n + GnQnG∗ n − Kp,nRe,nK∗ p,n

The only remaining question is how to start the recursion P0|−1 = x0 − ˆ x0|−12 = x02 = Π0

J. McNames

Portland State University ECE 539/639 Kalman Filter

Ver. 1.03

18

SLIDE 6

Example 1: Example Plots

2 4 6 8 10 −0.5 0.5 1 f:1.00 q:10.00 r:0.10 Position (m) 2 4 6 8 10 −1000 1000 Time (s) Measured Acceleration (m/s2)

J. McNames

Portland State University ECE 539/639 Kalman Filter

Ver. 1.03

23

Example 1: Accelerometer Suppose we wish to have a random walk model of an the position of an object and we have recordings from an accelerometer to estimate it’s position. Suppose the accelerometer is sampled at a rate of fs = 300 Hz. Our statistical model of the process is then ˙ p(t) = u(t) y(t) = d2p(t) dt2 + v(t) If we approximate the derivative in discrete time with dx(t) dt

t=nTs

≈ x(n) − x(n − 1) Ts d2x(t) dt2

t=nTs

≈ x(n) − 2x(n − 1) + x(n − 2) T 2

s

J. McNames

Portland State University ECE 539/639 Kalman Filter

Ver. 1.03

21

Example 1: Example Plots

1 2 3 4 5 6 7 8 9 10 −1.5 −1 −0.5 0.5 1 1.5 2 Time (s) Position (m) f:1.00 q:10.00 r:0.10 Actual Position Estimated Position Naive Estimate

J. McNames

Portland State University ECE 539/639 Kalman Filter

Ver. 1.03

24

Example 1: Discrete-Time State Space Model Then the discrete-time state space model of the process becomes ⎡ ⎣ p(n + 1) p(n) p(n − 1) ⎤ ⎦ = ⎡ ⎣ 1 1 1 ⎤ ⎦ ⎡ ⎣ p(n) p(n − 1) p(n − 2) ⎤ ⎦ + ⎡ ⎣ 1 ⎤ ⎦ u(n) y(n) = 1 T 2

s

1

−2 1

⎡

⎣ p(n) p(n − 1) p(n − 2) ⎤ ⎦ + v(n) Which is in our standard form. xn+1 = Fnxn + Gnun yn = Hnxn + vn

J. McNames

Portland State University ECE 539/639 Kalman Filter

Ver. 1.03

22

SLIDE 7

Example 1: Example Plots

1 2 3 4 5 6 7 8 9 10 −0.6 −0.4 −0.2 0.2 0.4 Time (s) Position (m) f:0.99 q:10.00 r:0.10 Actual Position Estimated Position Naive Estimate

J. McNames

Portland State University ECE 539/639 Kalman Filter

Ver. 1.03

27

Example 1: Example Plots

1 2 3 4 5 6 7 8 9 10 −1.5 −1 −0.5 0.5 1 1.5 2 Time (s) Position (m) f:1.00 q:10.00 r:0.10 Actual Position Estimated Position Naive Estimate

J. McNames

Portland State University ECE 539/639 Kalman Filter

Ver. 1.03

25

Example 1: Example Plots

1 2 3 4 5 6 7 8 9 10 −0.4 −0.2 0.2 0.4 0.6 Time (s) Position (m) f:0.99 q:10.00 r:0.10 Actual Position Estimated Position Naive Estimate

J. McNames

Portland State University ECE 539/639 Kalman Filter

Ver. 1.03

28

Example 1: Example Plots

1 2 3 4 5 6 7 8 9 10 −2 −1.5 −1 −0.5 0.5 1 Time (s) Position (m) f:1.00 q:10.00 r:0.10 Actual Position Estimated Position Naive Estimate

J. McNames

Portland State University ECE 539/639 Kalman Filter

Ver. 1.03

26

SLIDE 8

% Process Synthesis (simulation) %==================================================================== x = zeros(3,1); p = zeros(N,1); % Actual position y = zeros(N,1); % Measured acceleration mn = sqrt( mnv)*randn(N,1); % Measurement noise pn = sqrt(Ts*pnv)*randn(N,1); % Process noise for n = 1:N, p(n) = x(1); % Actual position y(n) = H*x + mn(n); % Measured acceleration x = F*x + G*pn(n); % Update the state vector end; %==================================================================== % Estimate the Position with the KF %==================================================================== xh = zeros(3,1); ph = zeros(N,1); % Estimated position phv = zeros(N,1); % Estimated error variance P = 1e-16*eye(3); phv(1) = P(1,1); for n=1:N-1, %---------------------------------------------------------------- % Calculations for time n %---------------------------------------------------------------- yh(n)= H*xh; % Predicted value of the signal e(n) = y(n) - yh(n); % Calculate the innovation at time n Re(n) = R + H*P*H’; % Update innovation covariance Kp = (F*P*H’ + G*S)*inv(Re(n)); % Kalman filter (prediction) coefficients %---------------------------------------------------------------- % Calculations for time n+1 %---------------------------------------------------------------- xh = F*xh + Kp*e(n); % Predicted state at time n+1|n+1

J. McNames

Portland State University ECE 539/639 Kalman Filter

Ver. 1.03

31

Example 1: Example Plots

1 2 3 4 5 6 7 8 9 10 −0.5 0.5 Time (s) Position (m) f:0.99 q:10.00 r:0.10 Actual Position Estimated Position Naive Estimate

J. McNames

Portland State University ECE 539/639 Kalman Filter

Ver. 1.03

29

P = F*P*F’ + G*Q*G’ - Kp*Re(n)*Kp’; % Calculate state error covariance matrix if eig(P)<0, warning(’Error covariance matrix is negative definite.’); end; %---------------------------------------------------------------- % Save Info %---------------------------------------------------------------- ph (n+1) = xh(1); phv(n+1) = P(1,1); end; ph2 = Ts^2*cumsum(cumsum(y)); %==================================================================== % Plot the Position and Acceleration Signals %==================================================================== figure; FigureSet(2,’LTX’); k = (1:N).’; t = (k-0.5)/fs; z = ones(N,1); phs = phv.^(1/2); ucb = norminv(0.975,ph,phs); % Upper confidence band lcb = norminv(0.025,ph,phs); % Lower confidence band h = patch([t;flipud(t)],[ucb;flipud(lcb)],’k’); set(h,’LineStyle’,’None’); set(h,’FaceColor’,[0.9 0.9 1.0]); hold on; h = plot3(t,p,z,’g’,t,ph,z,’b’,t,ph2,z,’r’); set(h,’LineWidth’,0.8); hold off; view(0,90); box off; xlim([0 T]); ylim([min([lcb;p;ph]) max([ucb;p;ph])]); AxisLines;

J. McNames

Portland State University ECE 539/639 Kalman Filter

Ver. 1.03

32

Example 1: MATLAB Code

clear all; close all; %==================================================================== % Parameters %==================================================================== fs = 100; % Sample rate (Hz) T = 10; % Duration of recording (seconds) pnv = 0.1; % Process noise variance (standard deviation of step size per second) mnv = 10; % Measurement noise variance (continuous time) fds = [0.99 1.00]; % Forgetting factors %==================================================================== % Preprocessing %==================================================================== Ts = 1/fs; % Sampling interval (s) N = round(fs*T); % No. samples for c0=1:length(fds), fd = fds(c0); %==================================================================== % State Model Parameters %==================================================================== F = [fd 0 0;1 0 0;0 1 0]; G = [1;0;0]; H = [1,-2,1]/Ts.^2; Q = Ts*pnv; R = mnv; S = 0; for c1=1:3, %====================================================================

J. McNames

Portland State University ECE 539/639 Kalman Filter

Ver. 1.03

30

SLIDE 9

Example 2: AR Modeling of an ARMA Process Use the Kalman filter to build an AR model of the ARMA process used in examples throughout ECE 538/638. Use a random walk state space model for the parameter vector with various measurement/process noise ratios. Qn = qI Rn = rI λ = q/r

J. McNames

Portland State University ECE 539/639 Kalman Filter

Ver. 1.03

35

ylabel(’Position (m)’); xlabel(’Time (s)’); AxisSet(8); title(sprintf(’$f$:%4.2f \\hspace{1em} $q$:%4.2f \\hspace{1em} $r$:%4.2f’,fd,mnv,pnv)); legend(h,’Actual Position’,’Estimated Position’,’Naive Estimate’); print(sprintf(’AccelerometerEstimate%d-%03d’,c1,round(fd*100)),’-depsc’); end; end; %==================================================================== % Plot the Position and Acceleration Signals %==================================================================== figure; FigureSet(1,’LTX’); k = 1:N; t = (k-0.5)/fs; subplot(2,1,1); h = plot(t,p,’g’); set(h,’LineWidth’,0.5); box off; AxisSet(8); ylabel(’Position (m)’); title(sprintf(’$f$:%4.2f \\hspace{1em} $q$:%4.2f \\hspace{1em} $r$:%4.2f’,fd,mnv,pnv)); xlim([0 T]); ylim([min(p) max(p)]); AxisLines; subplot(2,1,2); k = 0:N-1; h = plot(t,y,’b’); set(h,’LineWidth’,0.5); box off; xlabel(’Time (s)’); ylabel(’Measured Acceleration (m/s$^2$)’); xlim([0 T]); ylim([min(y) max(y)]); AxisLines; AxisSet(8);

J. McNames

Portland State University ECE 539/639 Kalman Filter

Ver. 1.03

33

Example 2: Pole-Zero Map

−1 −0.5 0.5 1 −1 −0.5 0.5 1

J. McNames

Portland State University ECE 539/639 Kalman Filter

Ver. 1.03

36

print(’AccelerometerSignals’,’-depsc’);

J. McNames

Portland State University ECE 539/639 Kalman Filter

Ver. 1.03

34

SLIDE 10

Example 2: Nonparametric Spectrogram

0.1 0.2 0.3 0.4 0.5 Frequency (Hz) 10 20 30 40 50 60 70 80 −500 500 Time (min) Signal 10 20 30 40 50 60 70 80 0.1 0.2 0.3 0.4 0.5

J. McNames

Portland State University ECE 539/639 Kalman Filter

Ver. 1.03

39

Example 2: Process Output

10 20 30 40 50 60 70 80 90 100 −600 −400 −200 200 400 Signal Sample Index Length:100

J. McNames

Portland State University ECE 539/639 Kalman Filter

Ver. 1.03

37

Example 2: Estimated PSD

0.1 0.2 0.3 0.4 Frequency (Hz) 10 20 30 40 50 60 70 80 −500 500 Time (min) Signal ℓ=10 q=1e-007 r=2.3e+004 ρ0= 1 fd= 1 10 20 30 40 50 60 70 80 0.1 0.2 0.3 0.4

J. McNames

Portland State University ECE 539/639 Kalman Filter

Ver. 1.03

40

Example 2: Process PSD

0.05 0.1 0.15 0.2 0.25 0.3 0.35 0.4 0.45 0.5 1 2 3 x 10

5

PSD True PSD 0.05 0.1 0.15 0.2 0.25 0.3 0.35 0.4 0.45 0.5 10 PSD Frequency (cycles/sample)

J. McNames

Portland State University ECE 539/639 Kalman Filter

Ver. 1.03

38

SLIDE 11

Example 2: Estimated PSD

0.1 0.2 0.3 0.4 Frequency (Hz) 10 20 30 40 50 60 70 80 −500 500 Time (min) Signal ℓ=10 q= 0.01 r=2.3e+004 ρ0= 1 fd= 1 10 20 30 40 50 60 70 80 0.1 0.2 0.3 0.4

J. McNames

Portland State University ECE 539/639 Kalman Filter

Ver. 1.03

43

Example 2: Estimated PSD

0.1 0.2 0.3 0.4 Frequency (Hz) 10 20 30 40 50 60 70 80 −500 500 Time (min) Signal ℓ=10 q=1e-005 r=2.3e+004 ρ0= 1 fd= 1 10 20 30 40 50 60 70 80 0.1 0.2 0.3 0.4

J. McNames

Portland State University ECE 539/639 Kalman Filter

Ver. 1.03

41

Example 2: Estimated PSD

0.1 0.2 0.3 0.4 Frequency (Hz) 10 20 30 40 50 60 70 80 −500 500 Time (min) Signal ℓ=10 q= 0.1 r=2.3e+004 ρ0= 1 fd= 1 10 20 30 40 50 60 70 80 0.1 0.2 0.3 0.4

J. McNames

Portland State University ECE 539/639 Kalman Filter

Ver. 1.03

44

Example 2: Estimated PSD

0.1 0.2 0.3 0.4 Frequency (Hz) 10 20 30 40 50 60 70 80 −500 500 Time (min) Signal ℓ=10 q=0.001 r=2.3e+004 ρ0= 1 fd= 1 10 20 30 40 50 60 70 80 0.1 0.2 0.3 0.4

J. McNames

Portland State University ECE 539/639 Kalman Filter

Ver. 1.03

42

SLIDE 12

ylim([-1.1 1.1]); AxisLines; box off; AxisSet(8); print -depsc ProcessPZMap; %==================================================================== % Plot Signal %==================================================================== n = 100; w = randn(Np+N,1); x = filter(b,a,w); % System with known PSD nx = length(x); y = x(nx-N+1:nx); % Eliminate start-up transient (make stationary) y = y; % Make unit variance figure; FigureSet(1,’LTX’); k = 0:N-1; h = stem(k,y); set(h,’Marker’,’.’); set(h,’LineWidth’,0.5); box off; ylabel(’Signal’); xlabel(’Sample Index’); title(sprintf(’Length:%d’,n)); xlim([0 n]); ylim([min(y) max(y)]); AxisSet(8); print -depsc ProcessOutput; %==================================================================== % Plot True PSD %==================================================================== figure; FigureSet(1,’LTX’); [R,w] = freqz(b,a,2^10);

J. McNames

Portland State University ECE 539/639 Kalman Filter

Ver. 1.03

47

Example 2: Estimated PSD

0.1 0.2 0.3 0.4 Frequency (Hz) 10 20 30 40 50 60 70 80 −500 500 Time (min) Signal ℓ=10 q=1e+005 r=2.3e+004 ρ0= 1 fd= 1 10 20 30 40 50 60 70 80 0.1 0.2 0.3 0.4

J. McNames

Portland State University ECE 539/639 Kalman Filter

Ver. 1.03

45

R2 = abs(R).^2; f = w/(2*pi); subplot(2,1,1); h = plot(f,R2,’r’); set(h,’LineWidth’,1.3); box off; ylabel(’PSD’); title(’True PSD’); xlim([0 0.5]); ylim([0 max(R2)*1.2]); subplot(2,1,2); h = semilogy(f,R2,’r’); set(h,’LineWidth’,1.3); box off; ylabel(’PSD’); xlim([0 0.5]); ylim([0.2*min(R2) max(R2)*5]); xlabel(’Frequency (cycles/sample)’); AxisSet(8); print -depsc ProcessPSD; %==================================================================== % Spectrogram %==================================================================== NonparametricSpectrogram(y,fs,200,[],2^9,500); FigureSet(1,’LTX’); AxisSet(8); print(’KFARSpectrogram’,’-depsc’); %==================================================================== % Kalman Filter Estimates %==================================================================== q = [1e-7 1e-5 1e-3 1e-2 1e-1 1e5]; for c1=1:length(q), figure; clf;

J. McNames

Portland State University ECE 539/639 Kalman Filter

Ver. 1.03

48

Example 2: MATLAB Code

clear; close all; %==================================================================== % User-Specified Parameters %==================================================================== fs = 1; % Sample rate (Hz) N = 5000; % Number of observations from the process Np = 500; % Number of samples to throw away to account for transient b = poly([-0.8,0.97*exp(j *pi/4),0.97*exp(-j *pi/4),... 0.97*exp(j *pi/6),0.97*exp(-j *pi/6)]); % Numerator coefficients a = poly([ 0.8,0.95*exp(j*3.0*pi/4),0.95*exp(-j*3.0*pi/4),... 0.95*exp(j*2.5*pi/4),0.95*exp(-j*2.5*pi/4)]); % Denominator coefficients b = b*sum(a)/sum(b); % Scale DC gain to 1 l = length(a)-1; % Order of the syste, m = l; % Dimension of the state/process noise p = 1; % Dimension of the observations (y) %==================================================================== % Plot the Pole-Zero Map %==================================================================== figure FigureSet(1,’LTX’); h = Circle; z = roots(b); p = roots(a); hold on; h2 = plot(real(z),imag(z),’bo’,real(p),imag(p),’rx’); hold off; axis square; xlim([-1.1 1.1]);

J. McNames

Portland State University ECE 539/639 Kalman Filter

Ver. 1.03

46

SLIDE 13

%

T. Kailath, A. H. Sayed, B. Hassibi, "Linear Estimation,"

% Prentice Hall, 2000. % % Version 1.00 JM % % See also Lowpass, ParametricSpectrogram, and KernelFilter. %==================================================================== % Error Checking %==================================================================== if nargin<1, help KalmanFilterAutoregressive; return; end; %==================================================================== % Author-Specified Parameters %==================================================================== Nf = 2^9; % Zero padding to use for PSD estimation Nt = 500; % No. times to evaluate the parametric spectrogram %==================================================================== % Process Function Arguments %==================================================================== fs = 1; if exist(’fsa’,’var’) & ~isempty(fsa), fs = fsa; end; l = 10; % Number of parameters (the letter l, not the number 1) if exist(’na’,’var’) && ~isempty(na), l = na; end; q = 0.1*var(y); % Process noise variance r = var(y); % Measurement noise variance p0 = 1e-10; % Initial state variance

J. McNames

Portland State University ECE 539/639 Kalman Filter

Ver. 1.03

51

KalmanFilterAutoregressive(y,fs,10,[q(c1) var(y) 1]); close; FigureSet(1,’LTX’); AxisSet(8); print(sprintf(’KFARQ%d’,round(log10(q(c1)))),’-depsc’); end;

J. McNames

Portland State University ECE 539/639 Kalman Filter

Ver. 1.03

49

if exist(’vra’,’var’) && ~isempty(vra), q = vra(1); r = vra(2); p0 = vra(3); end; fd = 1; % State transition matrix diagonal if exist(’fda’,’var’) && ~isempty(fda), fd = fda; end; sf = 0; % Smoothing flag. Default = no smoothing. if exist(’sfa’) & ~isempty(sfa), sf = sfa; end; pf = 0; % Default - no plotting if nargout==0, % Plot if no output arguments pf = 1; end; if exist(’pfa’) & ~isempty(pfa), pf = pfa; end; %==================================================================== % Preprocessing %==================================================================== y = y(:); ny = length(y); % No. samples m = l; % Dimension of the state/process noise = dimension of the state p = 1; % Dimension of the observations (y) F = fd; % State transition matrix (F) G = 1; % Process noise matrix %====================================================================

J. McNames

Portland State University ECE 539/639 Kalman Filter

Ver. 1.03

52

Example 2: MATLAB Code Continued

function [A,yh] = KalmanFilterAutoregressive(y,fsa,na,vra,fda,sfa,pfa); %KalmanFilterAutoregressive: Adaptive autoregressive model % % [A] = KalmanFilterAutoregressive(y,fs,n,vr,fd,sf,pf); % % y Input signal. % fs Sample rate (Hz). Default = 1 Hz. % n Model order. Default = 10. % vr Vector of process, measurement, and initial state variances. % Default = [0.1*var(y) var(y) 1]. % fd State transition matrix diagonal. Default = 1.00. % sf Smoothing flag: 0=none (default), 1=smooth. % pf Plot flag: 0=none (default), 1=screen. % % A Matrix containing the parameters versus time. % yh Predicted estimates of the output. % % Uses a random walk state update model with a constant-diagonal % covariance matrices for the initial state, measurement noise, % and process noise. This implementation uses the casual version %

f the Kalman filter. It is not robust or computationally

%

efficient. The input signal must be scalar (a vector).

% % When smoothing is applied, uses the Bryson-Frazier formulas as % given in Kailath et al. % % Example: Generate the parametric spectrogram of an intracranial % pressure signal using a Blackman-Harris window that is 45 s in % duration. % % load ICP.mat; % icpd = decimate(icp,10); % KalmanFilterAutoregressive(icpd,fs/10); %

J. McNames

Portland State University ECE 539/639 Kalman Filter

Ver. 1.03

50

SLIDE 14

la = Fp’*la + H’*inv(Re(n))*e(n); %---------------------------------------------------------------- % Save Info %---------------------------------------------------------------- X(:,n) = xh; % Store estimate at time n+1 yh(n) = H*xh; % Store predicted estimate at time n end; end; %==================================================================== % Post-processing %==================================================================== A = X; %==================================================================== % Plotting %==================================================================== if pf>=1, if pf~=2, figure; FigureSet(1); colormap(jet(256)); else figure(1); end; Pf = zeros(l,Nt); % Frequencies of the poles Ry = zeros(Nf,Nt); % Memory allocation for spectral estimate k = round(linspace(1,ny,Nt)); % Sample indices to evalute spectral estimate at for c1=1:Nt, Ry(:,c1) = abs(freqz(1,[1;-A(:,k(c1))],Nf,fs)); rts = roots([1;-A(:,k(c1))]); % Obtain the roots of the polynomial Pf(:,c1) = sort(angle(rts)*fs/(2*pi)); % Store the sorted frequencies end; t = (k-0.5)/fs; % Times of estimate

J. McNames

Portland State University ECE 539/639 Kalman Filter

Ver. 1.03

55

% Variable Initialization %==================================================================== Q = q*eye(m,m); % Covariance matrix for state/process noise R = r*eye(p,p); % Covariance matrix for observation noise S = zeros(m,p); % Cross-covariance matrix between measurement and observation noise P0 = p0*eye(l,l); % Initial state (x(0)) covariance matrix H = zeros(p,l); % Initial value of H(i) at i=0 xh = zeros(l,1); % Initial value of estimated state at time i=0 P = P0; X = zeros(l,ny); % Estimates of the state at all times K = zeros(l,ny); % Kalman gains Px = zeros(l,l,ny); % State error covariances yh = zeros(ny,1); % Predicted outputs e = zeros(ny,1); % Innovations %==================================================================== % Forward Recursions %==================================================================== for n=1:ny, %---------------------------------------------------------------- % Calculations for time n %---------------------------------------------------------------- id = n - (1:l); % Indices of observed signal to stuff in H(n) iv = find(id>0); % Indices that are non-negative H = zeros(1,l); H(iv) = y(id(iv)); % Update output matrix yh(n)= H*xh; % Predicted value of the signal e(n) = y(n) - yh(n); % Calculate the innovation at time n Re(n) = R + H*P*H’; % Update innovation covariance Kp = (F*P*H’ + G*S)*inv(Re(n)); % Kalman filter (prediction) coefficients %----------------------------------------------------------------

J. McNames

Portland State University ECE 539/639 Kalman Filter

Ver. 1.03

53

tl = ’Time (s)’; % Default label of time axis if t(end)>2000, t = t/60; tl = ’Time (min)’; end; f = (0:Nf-1)*fs/(2*Nf); % Frequencies of estimate %---------------------------------------------------------------- % Spectrogram %---------------------------------------------------------------- ha1 = axes(’Position’,[0.15 0.21 0.81 0.69]); s = reshape(abs(Ry),Nf*Nt,1); p = [0 prctile(s,97)]; imagesc(t,f,Ry,p); xlim([0 t(end)]); ylim([0 f(end)]); set(ha1,’YAxisLocation’,’Right’); set(ha1,’XAxisLocation’,’Top’); set(ha1,’YDir’,’normal’); AxisSet; title(sprintf(’$\\ell$=%d $q$=%5.2g $r$=%5.2g $\\rho_0$=%5.2g $f_d$=%5.4g’,l,q,r,p0,fd)); %---------------------------------------------------------------- % Colorbar %---------------------------------------------------------------- ha2 = axes(’Position’,[0.135 0.21 0.01 0.69]); colorbar(ha2); set(ha2,’Box’,’Off’) set(ha2,’YTick’,[]); %---------------------------------------------------------------- % Power Spectral Density %---------------------------------------------------------------- ha3 = axes(’Position’,[0.08 0.21 0.05 0.69]); psd = mean((Ry.^2).’);

J. McNames

Portland State University ECE 539/639 Kalman Filter

Ver. 1.03

56

% Calculations for time n+1 %---------------------------------------------------------------- xh = F*xh + Kp*e(n); % Predicted state at time n+1|n+1 P = F*P*F’ + G*Q*G’ - Kp*Re(n)*Kp’; % Calculate state error covariance matrix if eig(P)<0, warning(’Error covariance matrix is negative definite.’); end; %---------------------------------------------------------------- % Save Info %---------------------------------------------------------------- X(:,n) = xh; % Store estimate at time n+1 K(:,n) = Kp; Px(:,:,n) = P; % Store the state error covariance end; %==================================================================== % Backward Recursions (Bryson-Frazier Formulas) %==================================================================== if sf==1, Ps = zeros(l,l); % Smoothed state error covariance matrix L = zeros(l,l); la = zeros(l,1); for n=ny:-1:1, %---------------------------------------------------------------- % Calculations for time i %---------------------------------------------------------------- id = (n-1) - (0:l-1); % Indices of the observed signal to stuff in H(n) iv = find(id>0); H = zeros(1,l); H(iv) = y(id(iv)); % Calculate the output matrix H Pp = Px(:,:,n); % Recall the predicted state error covariance Fp = F - K(:,n)*H; xh = X(:,n) + Pp*Fp’*la;

J. McNames

Portland State University ECE 539/639 Kalman Filter

Ver. 1.03

54

SLIDE 15

Example 3: Chirp KF AR Model versus Spectrogram Compare the time-frequency resolution of the PSD estimated from the autoregressive parameters estimated with a Kalman filter to a nonparametric spectrogram.

J. McNames

Portland State University ECE 539/639 Kalman Filter

Ver. 1.03

59

h = plot(psd,f,’r’); %,[Smin Smax]); set(h,’LineWidth’,0.8); ylim([0 f(end)]); xlim([0 1.02*max(psd)]); ylabel(’Frequency (Hz)’); set(ha3,’XTick’,[]); %---------------------------------------------------------------- % Signal %---------------------------------------------------------------- ha4 = axes(’Position’,[0.15 0.10 0.81 0.10]); h = plot(t,y(k)); set(h,’LineWidth’,0.8); ymin = min(y); ymax = max(y); yrng = ymax-ymin; ymin = ymin - 0.02*yrng; ymax = ymax + 0.02*yrng; xlim([0 t(end)]); ylim([ymin ymax]); xlabel(tl); ylabel(’Signal’); axes(ha1); AxisSet(8); %---------------------------------------------------------------- % Spectrogram with Poles %---------------------------------------------------------------- % rg = prctile(reshape(Ry,prod(size(Ry)),1),[1,98]); % h = imagesc(t,f,Ry,rg); % % hold on; % % h = plot(t,Pf,’k.’); % % set(h,’MarkerSize’,5); % % h = plot(t,Pf,’w.’); % % set(h,’MarkerSize’,2);

J. McNames

Portland State University ECE 539/639 Kalman Filter

Ver. 1.03

57

Example 3: Chirp Nonparametric Spectrogram

0.1 0.2 0.3 0.4 0.5 Frequency (Hz) 10 20 30 40 50 60 70 80 −1 1 Time (min) Signal 10 20 30 40 50 60 70 80 0.1 0.2 0.3 0.4 0.5

J. McNames

Portland State University ECE 539/639 Kalman Filter

Ver. 1.03

60

% % hold off; % set(gca,’YDir’,’Normal’); % xlabel(’Time (s)’); % ylabel(’Frequency (Hz)’); % title(sprintf(’q=%5.2g r=%5.2g p0=%5.2g’,q,r,p0)); % box off; % AxisSet; % colorbar; if pf~=2, figure; FigureSet(2); colormap(jet(256)); else figure(2); end; h = imagesc(A(:,k)); set(gca,’YDir’,’Normal’); xlabel(’Time (s)’); ylabel(’Lag (k)’); box off; colorbar; AxisSet; end; %==================================================================== % Process Return Arguments %==================================================================== if nargout==0, clear(’A’,’yh’); end;

J. McNames

Portland State University ECE 539/639 Kalman Filter

Ver. 1.03

58

SLIDE 16

Example 3: Chirp KF Parametric Spectrogram

0.1 0.2 0.3 0.4 Frequency (Hz) 10 20 30 40 50 60 70 80 −1 1 Time (min) Signal ℓ=10 q=1e-005 r= 0.5 ρ0= 1 fd= 1 10 20 30 40 50 60 70 80 0.1 0.2 0.3 0.4

J. McNames

Portland State University ECE 539/639 Kalman Filter

Ver. 1.03

63

Example 3: Chirp KF Parametric Spectrogram

0.1 0.2 0.3 0.4 Frequency (Hz) 10 20 30 40 50 60 70 80 −1 1 Time (min) Signal ℓ=10 q=1e-007 r= 0.5 ρ0= 1 fd= 1 10 20 30 40 50 60 70 80 0.1 0.2 0.3 0.4

J. McNames

Portland State University ECE 539/639 Kalman Filter

Ver. 1.03

61

Example 3: Chirp KF Parametric Spectrogram

0.1 0.2 0.3 0.4 Frequency (Hz) 10 20 30 40 50 60 70 80 −1 1 Time (min) Signal ℓ=10 q=0.0001 r= 0.5 ρ0= 1 fd= 1 10 20 30 40 50 60 70 80 0.1 0.2 0.3 0.4

J. McNames

Portland State University ECE 539/639 Kalman Filter

Ver. 1.03

64

Example 3: Chirp KF Parametric Spectrogram

0.1 0.2 0.3 0.4 Frequency (Hz) 10 20 30 40 50 60 70 80 −1 1 Time (min) Signal ℓ=10 q=1e-006 r= 0.5 ρ0= 1 fd= 1 10 20 30 40 50 60 70 80 0.1 0.2 0.3 0.4

J. McNames

Portland State University ECE 539/639 Kalman Filter

Ver. 1.03

62

SLIDE 17

Example 3: MATLAB Code

clear; close all; %==================================================================== % User-Specified Parameters %==================================================================== fs = 1; % Sample rate (Hz) N = 5000; % Number of observations from the process Np = 500; % Number of samples to throw away to account for transient k1 = 1:N/4; t1 = (k1-0.5)/fs; yc = chirp(t1,0.05,t1(end),0.45); y = [yc fliplr(yc) yc fliplr(yc)]’; %==================================================================== % Spectrogram %==================================================================== NonparametricSpectrogram(y,fs,200); FigureSet(1,’LTX’); AxisSet(8); print(’KFChirpSpectrogram’,’-depsc’); %==================================================================== % Kalman Filter Estimates %==================================================================== q = [1e-7 1e-6 1e-5 1e-4 1e-3 1e5]; for c1=1:length(q), figure; clf; KalmanFilterAutoregressive(y,fs,10,[q(c1) var(y) 1]); close;

J. McNames

Portland State University ECE 539/639 Kalman Filter

Ver. 1.03

67

Example 3: Chirp KF Parametric Spectrogram

0.1 0.2 0.3 0.4 Frequency (Hz) 10 20 30 40 50 60 70 80 −1 1 Time (min) Signal ℓ=10 q=0.001 r= 0.5 ρ0= 1 fd= 1 10 20 30 40 50 60 70 80 0.1 0.2 0.3 0.4

J. McNames

Portland State University ECE 539/639 Kalman Filter

Ver. 1.03

65

FigureSet(1,’LTX’); AxisSet(8); print(sprintf(’KFChirpQ%d’,round(log10(q(c1)))),’-depsc’); end;

J. McNames

Portland State University ECE 539/639 Kalman Filter

Ver. 1.03

68

Example 3: Chirp KF Parametric Spectrogram

0.1 0.2 0.3 0.4 Frequency (Hz) 10 20 30 40 50 60 70 80 −1 1 Time (min) Signal ℓ=10 q=1e+005 r= 0.5 ρ0= 1 fd= 1 10 20 30 40 50 60 70 80 0.1 0.2 0.3 0.4

J. McNames

Portland State University ECE 539/639 Kalman Filter

Ver. 1.03

66

SLIDE 18

Example 4: ICP Non-zero Mean KF Parametric Spectrogram

1 2 3 4 5 6 Frequency (Hz) 200 400 600 800 1000 5 10 15 20 Time (s) Signal ℓ=50 q=0.048 r= 4.8 ρ0= 1 fd= 1 200 400 600 800 1000 1 2 3 4 5 6

J. McNames

Portland State University ECE 539/639 Kalman Filter

Ver. 1.03

71

Example 4: ICP KF AR Model versus Spectrogram Compare the time-frequency resolution of the PSD estimated from the autoregressive parameters estimated with a Kalman filter to a nonparametric spectrogram for estimating the PSD of an intracranial pressure signal.

J. McNames

Portland State University ECE 539/639 Kalman Filter

Ver. 1.03

69

Example 4: ICP KF Parametric Spectrogram

1 2 3 4 5 6 Frequency (Hz) 200 400 600 800 1000 −10 −5 5 Time (s) Signal ℓ=50 q=0.00048 r= 4.8 ρ0= 1 fd= 1 200 400 600 800 1000 1 2 3 4 5 6

J. McNames

Portland State University ECE 539/639 Kalman Filter

Ver. 1.03

72

Example 4: ICP Nonparametric Spectrogram

1 2 3 4 5 6 Frequency (Hz) 200 400 600 800 1000 5 10 15 20 Time (s) Signal 200 400 600 800 1000 1 2 3 4 5 6

J. McNames

Portland State University ECE 539/639 Kalman Filter

Ver. 1.03

70

SLIDE 19

Example 4: ICP KF Parametric Spectrogram

1 2 3 4 5 6 Frequency (Hz) 200 400 600 800 1000 −10 −5 5 Time (s) Signal ℓ=50 q=4.8e+002 r= 4.8 ρ0= 1 fd= 1 200 400 600 800 1000 1 2 3 4 5 6

J. McNames

Portland State University ECE 539/639 Kalman Filter

Ver. 1.03

75

Example 4: ICP KF Parametric Spectrogram

1 2 3 4 5 6 Frequency (Hz) 200 400 600 800 1000 −10 −5 5 Time (s) Signal ℓ=50 q=0.0048 r= 4.8 ρ0= 1 fd= 1 200 400 600 800 1000 1 2 3 4 5 6

J. McNames

Portland State University ECE 539/639 Kalman Filter

Ver. 1.03

73

Example 4: MATLAB Code

clear; close all; %==================================================================== % Load the Data %==================================================================== load(’ICP.mat’); y = decimate(icp,10); fs = fs/10; %==================================================================== % Spectrogram %==================================================================== NonparametricSpectrogram(y,fs,30); FigureSet(1,’LTX’); AxisSet(8); print(’KFICPSpectrogram’,’-depsc’); %==================================================================== % Parametric KF Spectrogram with Mean Intact %==================================================================== KalmanFilterAutoregressive(y,fs,50,[1e-2*var(y) var(y) 1]); close; FigureSet(1,’LTX’); AxisSet(8); print(’KFICPNonzeroMean’,’-depsc’); %==================================================================== % Kalman Filter Estimates %==================================================================== y = y - mean(y); q = [1e-4 1e-3 1e-2 1e2]; for c1=1:length(q),

J. McNames

Portland State University ECE 539/639 Kalman Filter

Ver. 1.03

76

Example 4: ICP KF Parametric Spectrogram

1 2 3 4 5 6 Frequency (Hz) 200 400 600 800 1000 −10 −5 5 Time (s) Signal ℓ=50 q=0.048 r= 4.8 ρ0= 1 fd= 1 200 400 600 800 1000 1 2 3 4 5 6

J. McNames

Portland State University ECE 539/639 Kalman Filter

Ver. 1.03

74

SLIDE 20

Predicted and Filtered Estimates

There are many equivalent forms of the Kalman filter
So far we have only derived the so-called covariance form
This creates one step predicted estimates of the state ˆ

xn|n−1

May be useful to have filtered estimates as well, ˆ

xn|n

Can also generalize so that we can obtain ˆ

xn|k for any k

For now will only focus on filtered estimates
J. McNames

Portland State University ECE 539/639 Kalman Filter

Ver. 1.03

79

figure; clf; KalmanFilterAutoregressive(y,fs,50,[q(c1)*var(y) var(y) 1]); close; FigureSet(1,’LTX’); AxisSet(8); print(sprintf(’KFICPQ%d’,round(log10(q(c1)))),’-depsc’); end;

J. McNames

Portland State University ECE 539/639 Kalman Filter

Ver. 1.03

77

Measurement and Time Updates Suppose we have computed the predicted estimate ˆ xn|n−1 and we wish to obtain the filtered estimate: ˆ xn|n =

n

k=0

xn, ekR−1

e,kek

= n−1

k=0

xn, ekR−1

e,kek

+ xn, ekR−1

e,nen

= ˆ xn|n−1 + xn, ekR−1

e,nen

= ˆ xn|n−1 + Kf,nen where Kf,n xn, ekR−1

e,n

Kf,n is called the filtered Kalman gain
But we still need to solve for xn, ek
J. McNames

Portland State University ECE 539/639 Kalman Filter

Ver. 1.03

80

AR Estimation Observations

Usually the user picks Rn = rI, Qn = qI, and Pi0 = ρI
The estimates are primarily determined by the ratio λ = q/r
The parameter ρ controls how quickly the model converges from

the initial parameter values

λ and ℓ control the bias-variance tradeoff
The estimate is surprisingly robust to these parameters
J. McNames

Portland State University ECE 539/639 Kalman Filter

Ver. 1.03

78

SLIDE 21

Time Updates: State Estimate Similarly, we can obtain ˆ xn+1|n and Pn+1|n from the filtered values ˆ xn|n and Pn|n xn+1|n = Fnxn + Gnun ˆ xn+1|n = Fn ˆ xn|n + Gn ˆ un|n ˆ un|n =

n

k=0

un, ekR−1

e,kek

= un, enR−1

e,nen

un, en = un, Hn ˜ xn + vn = un, vn = Sn ˆ un|n = SnR−1

e,nen

ˆ xn+1|n = Fn ˆ xn|n + GnSnR−1

e,nen

J. McNames

Portland State University ECE 539/639 Kalman Filter

Ver. 1.03

83

Solving for the Filtered Kalman Gain xn, ek = xn, Hn ˜ xn|n−1 + vn = xn, ˜ xn|n−1H∗

n + xn, vn

= ˆ xn|n−1 + ˜ xn|n−1, ˜ xn|n−1H∗

n

= ˜ xn|n−12H∗

n

= Pn|n−1H∗

n

Thus Kf,n xn, ekR−1

e,n = Pn|n−1H∗ nR−1 e,n

J. McNames

Portland State University ECE 539/639 Kalman Filter

Ver. 1.03

81

Time Updates: Covariance Pn+1|n = ˜ xn+1|n2 = xn+1 − ˆ xn+1|n2 = (Fnxn + Gnun) − (Fn ˆ xn|n + GnSnR−1

e,nen)2

= Fn ˜ xn|n + Gnun − GnSnR−1

e,nen2

Fn ˆ xn|n, Gnun = Fn(ˆ xn|n−1 + Kf,nen), Gnun = FnKf,nS∗

nG∗ n

Pn+1|n = FnPn|nF ∗

n + FnKf,nS∗ nG∗ n + GnSnK∗ f,nF ∗ n

+Gnun − SnR−1

e,nen2G∗ n

= FnPn|nF ∗

n + FnKf,nS∗ nG∗ n + GnSnK∗ f,nF ∗ n

+Gn(Qn − SnR−1

e,nS∗ n − SnR−1 e,nS∗ n + SnR−1 e,nS∗ n)G∗ n

= FnPn|nF ∗

n + FnKf,nS∗ nG∗ n + GnSnK∗ f,nF ∗ n

+Gn(Qn − SnR−1

e,nS∗ n)G∗ n

J. McNames

Portland State University ECE 539/639 Kalman Filter

Ver. 1.03

84

Filtered State Error Covariance We don’t need it, but it would also be nice to know the filtered state error covariance Pn|n ˜ xn|n2 = xn − ˆ xn|n2 = ˜ xn|n, xn − ˆ xn|n = ˜ xn|n, xn = xn − ˆ xn|n, xn = xn − ˆ xn|n−1 − Kf,nen, xn = xn − ˆ xn|n−1, xn − Kf,nen, xn = ˜ xn|n−1, xn − Kf,nHn ˜ xn|n−1 + vn, xn = Pn|n−1 − Kf,nHn˜ xn|n−1, xn = Pn|n−1 − Kf,nHnPn|n−1 = Pn|n−1 − Pn|n−1H∗

nR−1 e,nHnPn|n−1

J. McNames

Portland State University ECE 539/639 Kalman Filter

Ver. 1.03

82

SLIDE 22

Time and Measurement Updates 0 = ˆ x0|−1

m.u.

→ ˆ x0|0

t.u.

→ ˆ x1|0

m.u.

→ ˆ x1|1

t.u.

→ ˆ x2|1

m.u.

→ ˆ x2|2

t.u.

→ ˆ x3|2 . . . Π0 = P0|−1

m.u.

→ P0|0

t.u.

→ P1|0

m.u.

→ P1|1

t.u.

→ P2|1

m.u.

→ P2|2

t.u.

→ P3|2 . . .

Separating the measurement and time updates permits us to
btain two estimates of the state

– a priori estimate of ˆ xn|n−1 before the nth measurement has been obtained – a posteriori estimate ˆ xn|n after the measurement is taken

If a measurement is lost (drop out) can just use the predicted

value

J. McNames

Portland State University ECE 539/639 Kalman Filter

Ver. 1.03

87

Time Updates: Covariance Continued Pn+1|n = FnPn|nF ∗

n + FnKf,nS∗ nG∗ n + GnSnK∗ f,nF ∗ n

+Gn(Qn − SnR−1

e,nS∗ n)G∗ n

When Sn = 0, this simplifies considerably to Pn+1|n = FnPn|nF ∗

n + GnQnG∗ n

J. McNames

Portland State University ECE 539/639 Kalman Filter

Ver. 1.03