ZKHUH YDQG ZDUH '7 … · duh lqghshqghqw 0dufr /ryhud 3ureohpvwdwhphqw:hzdqwwr ghilqhhvwlpdwruviru...
Transcript of ZKHUH YDQG ZDUH '7 … · duh lqghshqghqw 0dufr /ryhud 3ureohpvwdwhphqw:hzdqwwr ghilqhhvwlpdwruviru...
![Page 1: ZKHUH YDQG ZDUH '7 … · duh lqghshqghqw 0dufr /ryhud 3ureohpvwdwhphqw:hzdqwwr ghilqhhvwlpdwruviru wkh vwdwh yhfwru[rq wkh edvlvri phdvxuhphqwvri wkh rxwsxw \ w ! 7 suhglfwlrqsureohp](https://reader033.fdocuments.in/reader033/viewer/2022050716/5e3a3c7a7cd027128d0ea6da/html5/thumbnails/1.jpg)
Kalman prediction and filteringMarco Lovera
Dipartimento di Scienze e Tecnologie Aerospaziali, Politecnico di Milano
![Page 2: ZKHUH YDQG ZDUH '7 … · duh lqghshqghqw 0dufr /ryhud 3ureohpvwdwhphqw:hzdqwwr ghilqhhvwlpdwruviru wkh vwdwh yhfwru[rq wkh edvlvri phdvxuhphqwvri wkh rxwsxw \ w ! 7 suhglfwlrqsureohp](https://reader033.fdocuments.in/reader033/viewer/2022050716/5e3a3c7a7cd027128d0ea6da/html5/thumbnails/2.jpg)
Marco Lovera
Problem statement
We start from the DT-DT problem, formulated as follows.The system under study is given by
where:
• v and w are DT white Gaussian noise processes with
• x1 is a Gaussian random variable:
• v, w and x1 are independent.
2
![Page 3: ZKHUH YDQG ZDUH '7 … · duh lqghshqghqw 0dufr /ryhud 3ureohpvwdwhphqw:hzdqwwr ghilqhhvwlpdwruviru wkh vwdwh yhfwru[rq wkh edvlvri phdvxuhphqwvri wkh rxwsxw \ w ! 7 suhglfwlrqsureohp](https://reader033.fdocuments.in/reader033/viewer/2022050716/5e3a3c7a7cd027128d0ea6da/html5/thumbnails/3.jpg)
Marco Lovera
Problem statement
We want to define estimators for the state vector x on the basis of measurements of the output y:
• t > T: prediction problem.
• t = T: filtering problem.
• 0 < t < T: smoothing problem.
We first consider the prediction problem, starting from one-step-ahead prediction.
3
![Page 4: ZKHUH YDQG ZDUH '7 … · duh lqghshqghqw 0dufr /ryhud 3ureohpvwdwhphqw:hzdqwwr ghilqhhvwlpdwruviru wkh vwdwh yhfwru[rq wkh edvlvri phdvxuhphqwvri wkh rxwsxw \ w ! 7 suhglfwlrqsureohp](https://reader033.fdocuments.in/reader033/viewer/2022050716/5e3a3c7a7cd027128d0ea6da/html5/thumbnails/4.jpg)
Marco Lovera
Background on LTI DT systems
Free response:
4
![Page 5: ZKHUH YDQG ZDUH '7 … · duh lqghshqghqw 0dufr /ryhud 3ureohpvwdwhphqw:hzdqwwr ghilqhhvwlpdwruviru wkh vwdwh yhfwru[rq wkh edvlvri phdvxuhphqwvri wkh rxwsxw \ w ! 7 suhglfwlrqsureohp](https://reader033.fdocuments.in/reader033/viewer/2022050716/5e3a3c7a7cd027128d0ea6da/html5/thumbnails/5.jpg)
Marco Lovera
Background on LTI DT systems
Forced response:
5
![Page 6: ZKHUH YDQG ZDUH '7 … · duh lqghshqghqw 0dufr /ryhud 3ureohpvwdwhphqw:hzdqwwr ghilqhhvwlpdwruviru wkh vwdwh yhfwru[rq wkh edvlvri phdvxuhphqwvri wkh rxwsxw \ w ! 7 suhglfwlrqsureohp](https://reader033.fdocuments.in/reader033/viewer/2022050716/5e3a3c7a7cd027128d0ea6da/html5/thumbnails/6.jpg)
Marco Lovera
Background on LTI DT systems
Comments:
• The free responseis linear in the initial state, so if the initial condition isGaussian the free response is also Gaussian for all t.
• The forced response
is linear in the samples of w(t), so if process noise is a Gaussian RP, the forced response is Gaussian for all t.
• Finally, since the system is linear, the response is the sum of free and forced and therefore is also Gaussian.
6
![Page 7: ZKHUH YDQG ZDUH '7 … · duh lqghshqghqw 0dufr /ryhud 3ureohpvwdwhphqw:hzdqwwr ghilqhhvwlpdwruviru wkh vwdwh yhfwru[rq wkh edvlvri phdvxuhphqwvri wkh rxwsxw \ w ! 7 suhglfwlrqsureohp](https://reader033.fdocuments.in/reader033/viewer/2022050716/5e3a3c7a7cd027128d0ea6da/html5/thumbnails/7.jpg)
Marco Lovera
Optimal one-step-ahead predictor
Using Bayes rule we can express the optimal one-step-
ahead state and output predictors as
We will use often the innovation
and the state prediction error
7
![Page 8: ZKHUH YDQG ZDUH '7 … · duh lqghshqghqw 0dufr /ryhud 3ureohpvwdwhphqw:hzdqwwr ghilqhhvwlpdwruviru wkh vwdwh yhfwru[rq wkh edvlvri phdvxuhphqwvri wkh rxwsxw \ w ! 7 suhglfwlrqsureohp](https://reader033.fdocuments.in/reader033/viewer/2022050716/5e3a3c7a7cd027128d0ea6da/html5/thumbnails/8.jpg)
Marco Lovera
Optimal one-step-ahead predictor
Consider first the output prediction:
The second term is zero, as:• y(N) is a function of v up to time N, of w up to time N-1 and of
x1.• v(N+1) is independent of
• previous samples of v and w• the initial state x1.
In other words, v(N+1) is unpredictable based on past data.
8
![Page 9: ZKHUH YDQG ZDUH '7 … · duh lqghshqghqw 0dufr /ryhud 3ureohpvwdwhphqw:hzdqwwr ghilqhhvwlpdwruviru wkh vwdwh yhfwru[rq wkh edvlvri phdvxuhphqwvri wkh rxwsxw \ w ! 7 suhglfwlrqsureohp](https://reader033.fdocuments.in/reader033/viewer/2022050716/5e3a3c7a7cd027128d0ea6da/html5/thumbnails/9.jpg)
Marco Lovera
Optimal one-step-ahead predictor
Therefore we have
Note that as in the Luenberger observer the prediction of the
output is expressed in terms of the prediction of the state
through the output matrix H.
9
![Page 10: ZKHUH YDQG ZDUH '7 … · duh lqghshqghqw 0dufr /ryhud 3ureohpvwdwhphqw:hzdqwwr ghilqhhvwlpdwruviru wkh vwdwh yhfwru[rq wkh edvlvri phdvxuhphqwvri wkh rxwsxw \ w ! 7 suhglfwlrqsureohp](https://reader033.fdocuments.in/reader033/viewer/2022050716/5e3a3c7a7cd027128d0ea6da/html5/thumbnails/10.jpg)
Marco Lovera
Optimal one-step-ahead predictor
Consider now the state prediction:
The second term can be written in terms of the innovation:
Next, we have to evaluate the two terms on the RHS.
10
![Page 11: ZKHUH YDQG ZDUH '7 … · duh lqghshqghqw 0dufr /ryhud 3ureohpvwdwhphqw:hzdqwwr ghilqhhvwlpdwruviru wkh vwdwh yhfwru[rq wkh edvlvri phdvxuhphqwvri wkh rxwsxw \ w ! 7 suhglfwlrqsureohp](https://reader033.fdocuments.in/reader033/viewer/2022050716/5e3a3c7a7cd027128d0ea6da/html5/thumbnails/11.jpg)
Marco Lovera
Optimal one-step-ahead predictor
The first term is given by:
Equivalently:
is zero, as:• y(N-1) is a function of v up to time N-1, of w up to time N-2 and
of x1.• w(N) is independent of
• previous samples of v and w• the initial state x1.
Therefore we get
11
![Page 12: ZKHUH YDQG ZDUH '7 … · duh lqghshqghqw 0dufr /ryhud 3ureohpvwdwhphqw:hzdqwwr ghilqhhvwlpdwruviru wkh vwdwh yhfwru[rq wkh edvlvri phdvxuhphqwvri wkh rxwsxw \ w ! 7 suhglfwlrqsureohp](https://reader033.fdocuments.in/reader033/viewer/2022050716/5e3a3c7a7cd027128d0ea6da/html5/thumbnails/12.jpg)
Marco Lovera
Optimal one-step-ahead predictor
Substituting:
Using the vector Bayes rule, the second term is given by
and to make it explicit we have to compute the two variance
matrices:
12
![Page 13: ZKHUH YDQG ZDUH '7 … · duh lqghshqghqw 0dufr /ryhud 3ureohpvwdwhphqw:hzdqwwr ghilqhhvwlpdwruviru wkh vwdwh yhfwru[rq wkh edvlvri phdvxuhphqwvri wkh rxwsxw \ w ! 7 suhglfwlrqsureohp](https://reader033.fdocuments.in/reader033/viewer/2022050716/5e3a3c7a7cd027128d0ea6da/html5/thumbnails/13.jpg)
Marco Lovera
Computation of
For the covariance between x(N+1) and e(N) we have
Computing the products:
13
![Page 14: ZKHUH YDQG ZDUH '7 … · duh lqghshqghqw 0dufr /ryhud 3ureohpvwdwhphqw:hzdqwwr ghilqhhvwlpdwruviru wkh vwdwh yhfwru[rq wkh edvlvri phdvxuhphqwvri wkh rxwsxw \ w ! 7 suhglfwlrqsureohp](https://reader033.fdocuments.in/reader033/viewer/2022050716/5e3a3c7a7cd027128d0ea6da/html5/thumbnails/14.jpg)
Marco Lovera
Computation of
Note that in
the second and the third terms are zero, so we have
To evaluate the expectation we re-write it as
and compute the products.
14
![Page 15: ZKHUH YDQG ZDUH '7 … · duh lqghshqghqw 0dufr /ryhud 3ureohpvwdwhphqw:hzdqwwr ghilqhhvwlpdwruviru wkh vwdwh yhfwru[rq wkh edvlvri phdvxuhphqwvri wkh rxwsxw \ w ! 7 suhglfwlrqsureohp](https://reader033.fdocuments.in/reader033/viewer/2022050716/5e3a3c7a7cd027128d0ea6da/html5/thumbnails/15.jpg)
Marco Lovera
Computation of
We get
which can be written in terms of the prediction error:
The second term is zero: the prediction error at time N is the
unpredictable part of x(N) and therefore is independent of
the prediction of x(N).
15
![Page 16: ZKHUH YDQG ZDUH '7 … · duh lqghshqghqw 0dufr /ryhud 3ureohpvwdwhphqw:hzdqwwr ghilqhhvwlpdwruviru wkh vwdwh yhfwru[rq wkh edvlvri phdvxuhphqwvri wkh rxwsxw \ w ! 7 suhglfwlrqsureohp](https://reader033.fdocuments.in/reader033/viewer/2022050716/5e3a3c7a7cd027128d0ea6da/html5/thumbnails/16.jpg)
Marco Lovera
Computation of
Therefore, we get
and letting
we have the final result
16
![Page 17: ZKHUH YDQG ZDUH '7 … · duh lqghshqghqw 0dufr /ryhud 3ureohpvwdwhphqw:hzdqwwr ghilqhhvwlpdwruviru wkh vwdwh yhfwru[rq wkh edvlvri phdvxuhphqwvri wkh rxwsxw \ w ! 7 suhglfwlrqsureohp](https://reader033.fdocuments.in/reader033/viewer/2022050716/5e3a3c7a7cd027128d0ea6da/html5/thumbnails/17.jpg)
Marco Lovera
Computation of
For the covariance between e(N) and e(N), recalling
that
we have
The cross-terms can be shown to be zero by means of the
usual arguments.
17
![Page 18: ZKHUH YDQG ZDUH '7 … · duh lqghshqghqw 0dufr /ryhud 3ureohpvwdwhphqw:hzdqwwr ghilqhhvwlpdwruviru wkh vwdwh yhfwru[rq wkh edvlvri phdvxuhphqwvri wkh rxwsxw \ w ! 7 suhglfwlrqsureohp](https://reader033.fdocuments.in/reader033/viewer/2022050716/5e3a3c7a7cd027128d0ea6da/html5/thumbnails/18.jpg)
Marco Lovera
Optimal one-step-ahead predictor
We now have:
where
and
therefore the complete predictor is
18
![Page 19: ZKHUH YDQG ZDUH '7 … · duh lqghshqghqw 0dufr /ryhud 3ureohpvwdwhphqw:hzdqwwr ghilqhhvwlpdwruviru wkh vwdwh yhfwru[rq wkh edvlvri phdvxuhphqwvri wkh rxwsxw \ w ! 7 suhglfwlrqsureohp](https://reader033.fdocuments.in/reader033/viewer/2022050716/5e3a3c7a7cd027128d0ea6da/html5/thumbnails/19.jpg)
Marco Lovera
Optimal one-step-ahead predictor
Letting
the gain of the predictor, we get
Recalling the definition of the innovation as
we recognize that the optimal predictor has the samestructure as the Luenberger observer.
19
![Page 20: ZKHUH YDQG ZDUH '7 … · duh lqghshqghqw 0dufr /ryhud 3ureohpvwdwhphqw:hzdqwwr ghilqhhvwlpdwruviru wkh vwdwh yhfwru[rq wkh edvlvri phdvxuhphqwvri wkh rxwsxw \ w ! 7 suhglfwlrqsureohp](https://reader033.fdocuments.in/reader033/viewer/2022050716/5e3a3c7a7cd027128d0ea6da/html5/thumbnails/20.jpg)
Marco Lovera
Optimal one-step-ahead predictor
Note however that unlike the Luenberger observer:
• The optimal gain K(N) determined using Bayes rule isNOT constant.
• The definition of the gain is not yet complete as we stillneed an update equation for P(N).
20
![Page 21: ZKHUH YDQG ZDUH '7 … · duh lqghshqghqw 0dufr /ryhud 3ureohpvwdwhphqw:hzdqwwr ghilqhhvwlpdwruviru wkh vwdwh yhfwru[rq wkh edvlvri phdvxuhphqwvri wkh rxwsxw \ w ! 7 suhglfwlrqsureohp](https://reader033.fdocuments.in/reader033/viewer/2022050716/5e3a3c7a7cd027128d0ea6da/html5/thumbnails/21.jpg)
Marco Lovera
Optimal one-step-ahead predictor
The update equation for P(N) can be derived starting from
the definition of prediction error:
which can be also written as
and recalling
21
![Page 22: ZKHUH YDQG ZDUH '7 … · duh lqghshqghqw 0dufr /ryhud 3ureohpvwdwhphqw:hzdqwwr ghilqhhvwlpdwruviru wkh vwdwh yhfwru[rq wkh edvlvri phdvxuhphqwvri wkh rxwsxw \ w ! 7 suhglfwlrqsureohp](https://reader033.fdocuments.in/reader033/viewer/2022050716/5e3a3c7a7cd027128d0ea6da/html5/thumbnails/22.jpg)
Marco Lovera
Optimal one-step-ahead predictor
Squaring
we get
Taking expectations of both sides:
as it can be shown that
22
![Page 23: ZKHUH YDQG ZDUH '7 … · duh lqghshqghqw 0dufr /ryhud 3ureohpvwdwhphqw:hzdqwwr ghilqhhvwlpdwruviru wkh vwdwh yhfwru[rq wkh edvlvri phdvxuhphqwvri wkh rxwsxw \ w ! 7 suhglfwlrqsureohp](https://reader033.fdocuments.in/reader033/viewer/2022050716/5e3a3c7a7cd027128d0ea6da/html5/thumbnails/23.jpg)
Marco Lovera
Optimal one-step-ahead predictor
The update equation for P(N)
can be also written as
where has been used.
Or, equivalently, as
This equation is known as the Difference Riccati Equation (DRE).
23
![Page 24: ZKHUH YDQG ZDUH '7 … · duh lqghshqghqw 0dufr /ryhud 3ureohpvwdwhphqw:hzdqwwr ghilqhhvwlpdwruviru wkh vwdwh yhfwru[rq wkh edvlvri phdvxuhphqwvri wkh rxwsxw \ w ! 7 suhglfwlrqsureohp](https://reader033.fdocuments.in/reader033/viewer/2022050716/5e3a3c7a7cd027128d0ea6da/html5/thumbnails/24.jpg)
Marco Lovera
Optimal one-step-ahead predictor
The last form
is interesting as it allows a simple interpretation.
• P(N) is a variance matrix, so it is positive semidefinite.
• Indeed the RHS is a sum of positive sign-definite terms.
• The first two (positive: variance increase) correspond to prediction, i.e., pure propagation of the variance on the system’s state equation.
• The last term (negative: variance reduction) corresponds to the correction, introduced by feedback of the innovation.
24
![Page 25: ZKHUH YDQG ZDUH '7 … · duh lqghshqghqw 0dufr /ryhud 3ureohpvwdwhphqw:hzdqwwr ghilqhhvwlpdwruviru wkh vwdwh yhfwru[rq wkh edvlvri phdvxuhphqwvri wkh rxwsxw \ w ! 7 suhglfwlrqsureohp](https://reader033.fdocuments.in/reader033/viewer/2022050716/5e3a3c7a7cd027128d0ea6da/html5/thumbnails/25.jpg)
Marco Lovera
Optimal one-step-ahead predictor
• The definition of the predictor is now complete.
• We just have to specify the initialisation for the predictionand for the variance of the prediction error.
• For the prediction, at time 1 we should condition for data at time 0, which is not available. Therefore
• For the Riccati equation:
25
![Page 26: ZKHUH YDQG ZDUH '7 … · duh lqghshqghqw 0dufr /ryhud 3ureohpvwdwhphqw:hzdqwwr ghilqhhvwlpdwruviru wkh vwdwh yhfwru[rq wkh edvlvri phdvxuhphqwvri wkh rxwsxw \ w ! 7 suhglfwlrqsureohp](https://reader033.fdocuments.in/reader033/viewer/2022050716/5e3a3c7a7cd027128d0ea6da/html5/thumbnails/26.jpg)
Marco Lovera
Optimal one-step-ahead predictor: summary
• System:
• State prediction:
• Gain and prediction error variance update:
26
![Page 27: ZKHUH YDQG ZDUH '7 … · duh lqghshqghqw 0dufr /ryhud 3ureohpvwdwhphqw:hzdqwwr ghilqhhvwlpdwruviru wkh vwdwh yhfwru[rq wkh edvlvri phdvxuhphqwvri wkh rxwsxw \ w ! 7 suhglfwlrqsureohp](https://reader033.fdocuments.in/reader033/viewer/2022050716/5e3a3c7a7cd027128d0ea6da/html5/thumbnails/27.jpg)
Marco Lovera
Optimal r-step-ahead predictor
We now turn to the problem of r-step-ahead prediction, i.e.,
the computation of
We have
Iterating down to N+1 we get
27
![Page 28: ZKHUH YDQG ZDUH '7 … · duh lqghshqghqw 0dufr /ryhud 3ureohpvwdwhphqw:hzdqwwr ghilqhhvwlpdwruviru wkh vwdwh yhfwru[rq wkh edvlvri phdvxuhphqwvri wkh rxwsxw \ w ! 7 suhglfwlrqsureohp](https://reader033.fdocuments.in/reader033/viewer/2022050716/5e3a3c7a7cd027128d0ea6da/html5/thumbnails/28.jpg)
Marco Lovera
Optimal r-step-ahead predictor
As a particular case note that if we evaluate
for r=1 we get
Therefore:
• if F is invertible we can easily solve the filtering problem from the one-step-ahead prediction:
• On the contrary if the filtered estimate is available, the one-step-ahead prediction is just a one-step propagation of the state equation.
28
![Page 29: ZKHUH YDQG ZDUH '7 … · duh lqghshqghqw 0dufr /ryhud 3ureohpvwdwhphqw:hzdqwwr ghilqhhvwlpdwruviru wkh vwdwh yhfwru[rq wkh edvlvri phdvxuhphqwvri wkh rxwsxw \ w ! 7 suhglfwlrqsureohp](https://reader033.fdocuments.in/reader033/viewer/2022050716/5e3a3c7a7cd027128d0ea6da/html5/thumbnails/29.jpg)
Marco Lovera
Application of optimal prediction: MPC
In Model Predictive Control (MPC):
• At each time instant the current output is measured and the state prediction is computed as function ot future outputs.
• A performance metric is optimised with respect to future control samples.
• The first sample of the computed control sequenceis applied.
• The whole procedure isrepeated at the subsequentstep (receding horizon principle).
29
![Page 30: ZKHUH YDQG ZDUH '7 … · duh lqghshqghqw 0dufr /ryhud 3ureohpvwdwhphqw:hzdqwwr ghilqhhvwlpdwruviru wkh vwdwh yhfwru[rq wkh edvlvri phdvxuhphqwvri wkh rxwsxw \ w ! 7 suhglfwlrqsureohp](https://reader033.fdocuments.in/reader033/viewer/2022050716/5e3a3c7a7cd027128d0ea6da/html5/thumbnails/30.jpg)
Marco Lovera
Optimal filtering
• For conventional real-time control however we are notinterested in estimating the future state but rather the current state.
• Therefore the problem we need to solve is filtering ratherthan prediction.
• As we will see, filtering can be solved easily by building on the optimal one-step-ahead predictor.
30
![Page 31: ZKHUH YDQG ZDUH '7 … · duh lqghshqghqw 0dufr /ryhud 3ureohpvwdwhphqw:hzdqwwr ghilqhhvwlpdwruviru wkh vwdwh yhfwru[rq wkh edvlvri phdvxuhphqwvri wkh rxwsxw \ w ! 7 suhglfwlrqsureohp](https://reader033.fdocuments.in/reader033/viewer/2022050716/5e3a3c7a7cd027128d0ea6da/html5/thumbnails/31.jpg)
Marco Lovera
Optimal filtering
We want to compute :
We have seen that
and it can be proved that
31
![Page 32: ZKHUH YDQG ZDUH '7 … · duh lqghshqghqw 0dufr /ryhud 3ureohpvwdwhphqw:hzdqwwr ghilqhhvwlpdwruviru wkh vwdwh yhfwru[rq wkh edvlvri phdvxuhphqwvri wkh rxwsxw \ w ! 7 suhglfwlrqsureohp](https://reader033.fdocuments.in/reader033/viewer/2022050716/5e3a3c7a7cd027128d0ea6da/html5/thumbnails/32.jpg)
Marco Lovera
Optimal filtering
Therefore, the optimal filter update is given by
where
Note that
32
![Page 33: ZKHUH YDQG ZDUH '7 … · duh lqghshqghqw 0dufr /ryhud 3ureohpvwdwhphqw:hzdqwwr ghilqhhvwlpdwruviru wkh vwdwh yhfwru[rq wkh edvlvri phdvxuhphqwvri wkh rxwsxw \ w ! 7 suhglfwlrqsureohp](https://reader033.fdocuments.in/reader033/viewer/2022050716/5e3a3c7a7cd027128d0ea6da/html5/thumbnails/33.jpg)
Marco Lovera
Optimal filter: (temporary) summary
• System:
• State filtering:
• Gain and prediction error variance update:
33
![Page 34: ZKHUH YDQG ZDUH '7 … · duh lqghshqghqw 0dufr /ryhud 3ureohpvwdwhphqw:hzdqwwr ghilqhhvwlpdwruviru wkh vwdwh yhfwru[rq wkh edvlvri phdvxuhphqwvri wkh rxwsxw \ w ! 7 suhglfwlrqsureohp](https://reader033.fdocuments.in/reader033/viewer/2022050716/5e3a3c7a7cd027128d0ea6da/html5/thumbnails/34.jpg)
Marco Lovera
Optimal filtering
• The previous expression is somewhat hybrid, in the sensethat it involves both filtered and predicted quantities.
• An expression of the filter in terms of the filter error
and its variance
can be derived, but it is very complicated and not suitable
for implementation.
34
![Page 35: ZKHUH YDQG ZDUH '7 … · duh lqghshqghqw 0dufr /ryhud 3ureohpvwdwhphqw:hzdqwwr ghilqhhvwlpdwruviru wkh vwdwh yhfwru[rq wkh edvlvri phdvxuhphqwvri wkh rxwsxw \ w ! 7 suhglfwlrqsureohp](https://reader033.fdocuments.in/reader033/viewer/2022050716/5e3a3c7a7cd027128d0ea6da/html5/thumbnails/35.jpg)
Marco Lovera
Optimal filtering
• In the following we will derive the so-called prediction-correction form for the optimal filter.
• This form combines predicted and filtered quantities in a systematic way.
35
![Page 36: ZKHUH YDQG ZDUH '7 … · duh lqghshqghqw 0dufr /ryhud 3ureohpvwdwhphqw:hzdqwwr ghilqhhvwlpdwruviru wkh vwdwh yhfwru[rq wkh edvlvri phdvxuhphqwvri wkh rxwsxw \ w ! 7 suhglfwlrqsureohp](https://reader033.fdocuments.in/reader033/viewer/2022050716/5e3a3c7a7cd027128d0ea6da/html5/thumbnails/36.jpg)
Marco Lovera
Predictor/corrector Kalman filter
Recall that
so we can obtain the prediction at N+1 from the filtered
estimate at time N.
The new filtered estimate can be seen as a correction based
on the measurement at time N:
36
![Page 37: ZKHUH YDQG ZDUH '7 … · duh lqghshqghqw 0dufr /ryhud 3ureohpvwdwhphqw:hzdqwwr ghilqhhvwlpdwruviru wkh vwdwh yhfwru[rq wkh edvlvri phdvxuhphqwvri wkh rxwsxw \ w ! 7 suhglfwlrqsureohp](https://reader033.fdocuments.in/reader033/viewer/2022050716/5e3a3c7a7cd027128d0ea6da/html5/thumbnails/37.jpg)
Marco Lovera
Predictor/corrector Kalman filter
For variances: we have from the Riccati equation that
and using :
Based on
it can be proved that
37
![Page 38: ZKHUH YDQG ZDUH '7 … · duh lqghshqghqw 0dufr /ryhud 3ureohpvwdwhphqw:hzdqwwr ghilqhhvwlpdwruviru wkh vwdwh yhfwru[rq wkh edvlvri phdvxuhphqwvri wkh rxwsxw \ w ! 7 suhglfwlrqsureohp](https://reader033.fdocuments.in/reader033/viewer/2022050716/5e3a3c7a7cd027128d0ea6da/html5/thumbnails/38.jpg)
Marco Lovera
Predictor/corrector Kalman filter
Therefore if the filter error variance from the previous time
instant is known, then the prediction error variance is
and the updated filter error variance is:
which recalling
can be simplified to
38
![Page 39: ZKHUH YDQG ZDUH '7 … · duh lqghshqghqw 0dufr /ryhud 3ureohpvwdwhphqw:hzdqwwr ghilqhhvwlpdwruviru wkh vwdwh yhfwru[rq wkh edvlvri phdvxuhphqwvri wkh rxwsxw \ w ! 7 suhglfwlrqsureohp](https://reader033.fdocuments.in/reader033/viewer/2022050716/5e3a3c7a7cd027128d0ea6da/html5/thumbnails/39.jpg)
Marco Lovera
Optimal filtering
• In the predictor/corrector form a slightly different notationis used:
39
![Page 40: ZKHUH YDQG ZDUH '7 … · duh lqghshqghqw 0dufr /ryhud 3ureohpvwdwhphqw:hzdqwwr ghilqhhvwlpdwruviru wkh vwdwh yhfwru[rq wkh edvlvri phdvxuhphqwvri wkh rxwsxw \ w ! 7 suhglfwlrqsureohp](https://reader033.fdocuments.in/reader033/viewer/2022050716/5e3a3c7a7cd027128d0ea6da/html5/thumbnails/40.jpg)
Marco Lovera
Predictor/corrector Kalman filter
State estimate and error covariance extrapolation:
Gain update:
State estimate update and error covariance update:
40
![Page 41: ZKHUH YDQG ZDUH '7 … · duh lqghshqghqw 0dufr /ryhud 3ureohpvwdwhphqw:hzdqwwr ghilqhhvwlpdwruviru wkh vwdwh yhfwru[rq wkh edvlvri phdvxuhphqwvri wkh rxwsxw \ w ! 7 suhglfwlrqsureohp](https://reader033.fdocuments.in/reader033/viewer/2022050716/5e3a3c7a7cd027128d0ea6da/html5/thumbnails/41.jpg)
Marco Lovera
Prediction and filtering: generalisations
• The results on Kalman prediction and filtering have beenderived under some simplifying assumptions for the sakeof simplicity.
• Some of the assumptions can be removed, so that the results have more general validity.
41
![Page 42: ZKHUH YDQG ZDUH '7 … · duh lqghshqghqw 0dufr /ryhud 3ureohpvwdwhphqw:hzdqwwr ghilqhhvwlpdwruviru wkh vwdwh yhfwru[rq wkh edvlvri phdvxuhphqwvri wkh rxwsxw \ w ! 7 suhglfwlrqsureohp](https://reader033.fdocuments.in/reader033/viewer/2022050716/5e3a3c7a7cd027128d0ea6da/html5/thumbnails/42.jpg)
Marco Lovera
Including control inputs
• Consider a plant model which includes a control input
• Then the input can be included in the prediction/filteringapproach developed so far just like in the Luenbergerobserver problem.
42
![Page 43: ZKHUH YDQG ZDUH '7 … · duh lqghshqghqw 0dufr /ryhud 3ureohpvwdwhphqw:hzdqwwr ghilqhhvwlpdwruviru wkh vwdwh yhfwru[rq wkh edvlvri phdvxuhphqwvri wkh rxwsxw \ w ! 7 suhglfwlrqsureohp](https://reader033.fdocuments.in/reader033/viewer/2022050716/5e3a3c7a7cd027128d0ea6da/html5/thumbnails/43.jpg)
Marco Lovera
Including control inputs: predictor
• System:
• State prediction:
• Gain and prediction error variance update:
43
![Page 44: ZKHUH YDQG ZDUH '7 … · duh lqghshqghqw 0dufr /ryhud 3ureohpvwdwhphqw:hzdqwwr ghilqhhvwlpdwruviru wkh vwdwh yhfwru[rq wkh edvlvri phdvxuhphqwvri wkh rxwsxw \ w ! 7 suhglfwlrqsureohp](https://reader033.fdocuments.in/reader033/viewer/2022050716/5e3a3c7a7cd027128d0ea6da/html5/thumbnails/44.jpg)
Marco Lovera
Including control inputs: filter
State estimate and error covariance extrapolation:
Gain update:
State estimate update and error covariance update:
44
![Page 45: ZKHUH YDQG ZDUH '7 … · duh lqghshqghqw 0dufr /ryhud 3ureohpvwdwhphqw:hzdqwwr ghilqhhvwlpdwruviru wkh vwdwh yhfwru[rq wkh edvlvri phdvxuhphqwvri wkh rxwsxw \ w ! 7 suhglfwlrqsureohp](https://reader033.fdocuments.in/reader033/viewer/2022050716/5e3a3c7a7cd027128d0ea6da/html5/thumbnails/45.jpg)
Marco Lovera
Time-varying dynamics and noise variances
• The assumption of LTI dynamics can be relaxed.
• The above results on prediction/filtering hold unchangedin the case of a time-varying linear system:
• In particular, both time-varying dynamics and time-varyingnoise variances can be handled in the Kalman filteringframework.
45
![Page 46: ZKHUH YDQG ZDUH '7 … · duh lqghshqghqw 0dufr /ryhud 3ureohpvwdwhphqw:hzdqwwr ghilqhhvwlpdwruviru wkh vwdwh yhfwru[rq wkh edvlvri phdvxuhphqwvri wkh rxwsxw \ w ! 7 suhglfwlrqsureohp](https://reader033.fdocuments.in/reader033/viewer/2022050716/5e3a3c7a7cd027128d0ea6da/html5/thumbnails/46.jpg)
Marco Lovera
Time-varying dynamics and noisevariances: predictor
• State prediction:
• Gain and prediction error variance update:
46
![Page 47: ZKHUH YDQG ZDUH '7 … · duh lqghshqghqw 0dufr /ryhud 3ureohpvwdwhphqw:hzdqwwr ghilqhhvwlpdwruviru wkh vwdwh yhfwru[rq wkh edvlvri phdvxuhphqwvri wkh rxwsxw \ w ! 7 suhglfwlrqsureohp](https://reader033.fdocuments.in/reader033/viewer/2022050716/5e3a3c7a7cd027128d0ea6da/html5/thumbnails/47.jpg)
Marco Lovera
Time-varying dynamics and noisevariances: filter
State estimate and error covariance extrapolation:
Gain update:
State estimate update and error covariance update:
47
![Page 48: ZKHUH YDQG ZDUH '7 … · duh lqghshqghqw 0dufr /ryhud 3ureohpvwdwhphqw:hzdqwwr ghilqhhvwlpdwruviru wkh vwdwh yhfwru[rq wkh edvlvri phdvxuhphqwvri wkh rxwsxw \ w ! 7 suhglfwlrqsureohp](https://reader033.fdocuments.in/reader033/viewer/2022050716/5e3a3c7a7cd027128d0ea6da/html5/thumbnails/48.jpg)
Marco Lovera
Correlated noise processes
• In the derivation of the predictor and filter we assumedthat
• Also this assumption can be relaxed and the derivedsolutions generalised to the case when
48
![Page 49: ZKHUH YDQG ZDUH '7 … · duh lqghshqghqw 0dufr /ryhud 3ureohpvwdwhphqw:hzdqwwr ghilqhhvwlpdwruviru wkh vwdwh yhfwru[rq wkh edvlvri phdvxuhphqwvri wkh rxwsxw \ w ! 7 suhglfwlrqsureohp](https://reader033.fdocuments.in/reader033/viewer/2022050716/5e3a3c7a7cd027128d0ea6da/html5/thumbnails/49.jpg)
Marco Lovera
Steady-state Kalman predictor/filter
• The optimal solution derived so far has as main downsidethat the gain K(N) is time-varying even in the LTI case.
• This implies that the implementation requires the propagation of P(N) besides the propagation of the estimate.
• There is evidence however that in many problems after a transient the gain converges to a constant value.
49
![Page 50: ZKHUH YDQG ZDUH '7 … · duh lqghshqghqw 0dufr /ryhud 3ureohpvwdwhphqw:hzdqwwr ghilqhhvwlpdwruviru wkh vwdwh yhfwru[rq wkh edvlvri phdvxuhphqwvri wkh rxwsxw \ w ! 7 suhglfwlrqsureohp](https://reader033.fdocuments.in/reader033/viewer/2022050716/5e3a3c7a7cd027128d0ea6da/html5/thumbnails/50.jpg)
Marco Lovera
Steady-state Kalman predictor/filter
If the gain K(N) converges to a constant:
then the predictor
is called the steady-state predictor.
Note that substituting e(N) we have
which is a LTI system.
50
![Page 51: ZKHUH YDQG ZDUH '7 … · duh lqghshqghqw 0dufr /ryhud 3ureohpvwdwhphqw:hzdqwwr ghilqhhvwlpdwruviru wkh vwdwh yhfwru[rq wkh edvlvri phdvxuhphqwvri wkh rxwsxw \ w ! 7 suhglfwlrqsureohp](https://reader033.fdocuments.in/reader033/viewer/2022050716/5e3a3c7a7cd027128d0ea6da/html5/thumbnails/51.jpg)
Marco Lovera
Steady-state Kalman predictor/filter
The following questions then arise:
• Under which conditions does the gain converge?
• Does the gain converge to a stabilising value?
• If it does, how do we compute the steady-state gain?
• What is the actual performance loss incurred by considering the steady-state Kalman predictor/filter?
Recall that
Therefore the convergence of the gain depends on the convergence of P(N).
51
![Page 52: ZKHUH YDQG ZDUH '7 … · duh lqghshqghqw 0dufr /ryhud 3ureohpvwdwhphqw:hzdqwwr ghilqhhvwlpdwruviru wkh vwdwh yhfwru[rq wkh edvlvri phdvxuhphqwvri wkh rxwsxw \ w ! 7 suhglfwlrqsureohp](https://reader033.fdocuments.in/reader033/viewer/2022050716/5e3a3c7a7cd027128d0ea6da/html5/thumbnails/52.jpg)
Marco Lovera
Steady-state Kalman predictor/filter
• Consider initially the case in which the system isasymptotically stable.
• Then, we study the variance of the state sequence.
• From
• And taking the expectation:
52
![Page 53: ZKHUH YDQG ZDUH '7 … · duh lqghshqghqw 0dufr /ryhud 3ureohpvwdwhphqw:hzdqwwr ghilqhhvwlpdwruviru wkh vwdwh yhfwru[rq wkh edvlvri phdvxuhphqwvri wkh rxwsxw \ w ! 7 suhglfwlrqsureohp](https://reader033.fdocuments.in/reader033/viewer/2022050716/5e3a3c7a7cd027128d0ea6da/html5/thumbnails/53.jpg)
Marco Lovera
Steady-state Kalman predictor/filter
Comparing
to the Riccati equation
we conclude that
But if the system is stable then
and therefore also
53
![Page 54: ZKHUH YDQG ZDUH '7 … · duh lqghshqghqw 0dufr /ryhud 3ureohpvwdwhphqw:hzdqwwr ghilqhhvwlpdwruviru wkh vwdwh yhfwru[rq wkh edvlvri phdvxuhphqwvri wkh rxwsxw \ w ! 7 suhglfwlrqsureohp](https://reader033.fdocuments.in/reader033/viewer/2022050716/5e3a3c7a7cd027128d0ea6da/html5/thumbnails/54.jpg)
Marco Lovera
Steady-state Kalman predictor/filter
Based on this argument it can be proved that:
If the system is asymptotically stable then
• The solution of the DRE converges to
and the limit is independent of the initial condition.
• The corresponding steady-state predictor is asymptoticallystable.
54
![Page 55: ZKHUH YDQG ZDUH '7 … · duh lqghshqghqw 0dufr /ryhud 3ureohpvwdwhphqw:hzdqwwr ghilqhhvwlpdwruviru wkh vwdwh yhfwru[rq wkh edvlvri phdvxuhphqwvri wkh rxwsxw \ w ! 7 suhglfwlrqsureohp](https://reader033.fdocuments.in/reader033/viewer/2022050716/5e3a3c7a7cd027128d0ea6da/html5/thumbnails/55.jpg)
Marco Lovera
Steady-state Kalman predictor/filter
How does one compute the steady-state gain?
If then by definition at steady state we have
and therefore the DRE
reduces to the Discrete Algebraic Riccati Equation (DARE):
55
![Page 56: ZKHUH YDQG ZDUH '7 … · duh lqghshqghqw 0dufr /ryhud 3ureohpvwdwhphqw:hzdqwwr ghilqhhvwlpdwruviru wkh vwdwh yhfwru[rq wkh edvlvri phdvxuhphqwvri wkh rxwsxw \ w ! 7 suhglfwlrqsureohp](https://reader033.fdocuments.in/reader033/viewer/2022050716/5e3a3c7a7cd027128d0ea6da/html5/thumbnails/56.jpg)
Marco Lovera
Steady-state Kalman predictor/filter
Under stability assumptions, the DARE has a unique positive definite solution from which the steady-state gain can be computed:
56
![Page 57: ZKHUH YDQG ZDUH '7 … · duh lqghshqghqw 0dufr /ryhud 3ureohpvwdwhphqw:hzdqwwr ghilqhhvwlpdwruviru wkh vwdwh yhfwru[rq wkh edvlvri phdvxuhphqwvri wkh rxwsxw \ w ! 7 suhglfwlrqsureohp](https://reader033.fdocuments.in/reader033/viewer/2022050716/5e3a3c7a7cd027128d0ea6da/html5/thumbnails/57.jpg)
Marco Lovera
Steady-state Kalman predictor/filter
Example: the scalar case.
In the case of a first order model, the DARE reduces to
57
![Page 58: ZKHUH YDQG ZDUH '7 … · duh lqghshqghqw 0dufr /ryhud 3ureohpvwdwhphqw:hzdqwwr ghilqhhvwlpdwruviru wkh vwdwh yhfwru[rq wkh edvlvri phdvxuhphqwvri wkh rxwsxw \ w ! 7 suhglfwlrqsureohp](https://reader033.fdocuments.in/reader033/viewer/2022050716/5e3a3c7a7cd027128d0ea6da/html5/thumbnails/58.jpg)
Marco Lovera
Settling time for the response of a first orderDT system
The state equation
has a free response given by
which letting
becomes
and therefore the settling time in
steps is
a round(tA)
0.9 47
0.8 22
0.7 14
0.6 10
0.5 7
0.4 5
0.3 4
0.2 3
0.1 2
0.01 1
58
![Page 59: ZKHUH YDQG ZDUH '7 … · duh lqghshqghqw 0dufr /ryhud 3ureohpvwdwhphqw:hzdqwwr ghilqhhvwlpdwruviru wkh vwdwh yhfwru[rq wkh edvlvri phdvxuhphqwvri wkh rxwsxw \ w ! 7 suhglfwlrqsureohp](https://reader033.fdocuments.in/reader033/viewer/2022050716/5e3a3c7a7cd027128d0ea6da/html5/thumbnails/59.jpg)
Marco Lovera
Steady-state Kalman predictor/filter
Example: the scalar case.
Fix for example and study the effect of W and V:
For W=1:
V P K tA1 1.13 0.2656 0.2344 3.4
0.1 1.02 0.4555 0.0445 1.6
0.01 1.002 0.495 0.0049 0.94
59
![Page 60: ZKHUH YDQG ZDUH '7 … · duh lqghshqghqw 0dufr /ryhud 3ureohpvwdwhphqw:hzdqwwr ghilqhhvwlpdwruviru wkh vwdwh yhfwru[rq wkh edvlvri phdvxuhphqwvri wkh rxwsxw \ w ! 7 suhglfwlrqsureohp](https://reader033.fdocuments.in/reader033/viewer/2022050716/5e3a3c7a7cd027128d0ea6da/html5/thumbnails/60.jpg)
Marco Lovera
Steady-state Kalman predictor/filter
Example: the scalar case.
Fix for example and study the effect of W and V:
For V=1:
W P K tA1 1.13 0.2656 0.2344 3.4
0.1 1.18 0.0569 0.4431 6.1
0.01 0.01 0.0066 0.4934 7
60
![Page 61: ZKHUH YDQG ZDUH '7 … · duh lqghshqghqw 0dufr /ryhud 3ureohpvwdwhphqw:hzdqwwr ghilqhhvwlpdwruviru wkh vwdwh yhfwru[rq wkh edvlvri phdvxuhphqwvri wkh rxwsxw \ w ! 7 suhglfwlrqsureohp](https://reader033.fdocuments.in/reader033/viewer/2022050716/5e3a3c7a7cd027128d0ea6da/html5/thumbnails/61.jpg)
Marco Lovera
Steady-state Kalman predictor/filter
• In many problems however the model is not asymptoticallystable.
• For example, in the single-axis attitude estimation problemthe dynamic matrix is given by
which has both eigenvalues equal to 1.
• Nonetheless we have seen that the filter converges to a stabilising gain.
61
![Page 62: ZKHUH YDQG ZDUH '7 … · duh lqghshqghqw 0dufr /ryhud 3ureohpvwdwhphqw:hzdqwwr ghilqhhvwlpdwruviru wkh vwdwh yhfwru[rq wkh edvlvri phdvxuhphqwvri wkh rxwsxw \ w ! 7 suhglfwlrqsureohp](https://reader033.fdocuments.in/reader033/viewer/2022050716/5e3a3c7a7cd027128d0ea6da/html5/thumbnails/62.jpg)
Marco Lovera
Steady-state Kalman predictor/filter
• As in the case of the Luenberger observer the structuralproperties of the model play a role.
• It is intuitive that for closed-loop stability the observabilityof (F, H) is important.
• This however is not the only condition: we look at thisusing an example.
62
![Page 63: ZKHUH YDQG ZDUH '7 … · duh lqghshqghqw 0dufr /ryhud 3ureohpvwdwhphqw:hzdqwwr ghilqhhvwlpdwruviru wkh vwdwh yhfwru[rq wkh edvlvri phdvxuhphqwvri wkh rxwsxw \ w ! 7 suhglfwlrqsureohp](https://reader033.fdocuments.in/reader033/viewer/2022050716/5e3a3c7a7cd027128d0ea6da/html5/thumbnails/63.jpg)
Marco Lovera
Steady-state Kalman predictor/filter
Consider again the scalar case and assume that W=0 (no process noise in the state equation) and P1=0.
The scalar DARE
in this case reduces to
which has as roots
63
![Page 64: ZKHUH YDQG ZDUH '7 … · duh lqghshqghqw 0dufr /ryhud 3ureohpvwdwhphqw:hzdqwwr ghilqhhvwlpdwruviru wkh vwdwh yhfwru[rq wkh edvlvri phdvxuhphqwvri wkh rxwsxw \ w ! 7 suhglfwlrqsureohp](https://reader033.fdocuments.in/reader033/viewer/2022050716/5e3a3c7a7cd027128d0ea6da/html5/thumbnails/64.jpg)
Marco Lovera
Steady-state Kalman predictor/filter
• For an unstable system the optimal solution is P=0.
• This is consistent with the assumptions: if the state equation is deterministic then we expect null predictionerror.
• This however implies K=0 and therefore (F-KH)=F will be unstable.
• If however we add a small process noise then the nullsolution of the DARE vanishes and we get a non-zero gain.
64
![Page 65: ZKHUH YDQG ZDUH '7 … · duh lqghshqghqw 0dufr /ryhud 3ureohpvwdwhphqw:hzdqwwr ghilqhhvwlpdwruviru wkh vwdwh yhfwru[rq wkh edvlvri phdvxuhphqwvri wkh rxwsxw \ w ! 7 suhglfwlrqsureohp](https://reader033.fdocuments.in/reader033/viewer/2022050716/5e3a3c7a7cd027128d0ea6da/html5/thumbnails/65.jpg)
Marco Lovera
Steady-state Kalman predictor/filter
Based on these arguments it can be proved that:
If the (F, H) pair is observable and the (F, G) pair isreachable, where then
• The solution of the DRE converges to
and the limit is independent of the initial condition.
• The corresponding steady-state predictor is asymptoticallystable.
65
![Page 66: ZKHUH YDQG ZDUH '7 … · duh lqghshqghqw 0dufr /ryhud 3ureohpvwdwhphqw:hzdqwwr ghilqhhvwlpdwruviru wkh vwdwh yhfwru[rq wkh edvlvri phdvxuhphqwvri wkh rxwsxw \ w ! 7 suhglfwlrqsureohp](https://reader033.fdocuments.in/reader033/viewer/2022050716/5e3a3c7a7cd027128d0ea6da/html5/thumbnails/66.jpg)
Marco Lovera
The CT-DT Kalman filter
• We now turn to the case in which the system for which wewant to estimate the state has continuous-time dynamicsand a discrete-time measurement equation
66
![Page 67: ZKHUH YDQG ZDUH '7 … · duh lqghshqghqw 0dufr /ryhud 3ureohpvwdwhphqw:hzdqwwr ghilqhhvwlpdwruviru wkh vwdwh yhfwru[rq wkh edvlvri phdvxuhphqwvri wkh rxwsxw \ w ! 7 suhglfwlrqsureohp](https://reader033.fdocuments.in/reader033/viewer/2022050716/5e3a3c7a7cd027128d0ea6da/html5/thumbnails/67.jpg)
Marco Lovera
The CT-DT Kalman filter
• To use the results on the DT solution we have to relate the CT state equation and the DT one.
• We do it using simple Euler integration:
67