Predictive Localization estimate of the state
3
1 IAR - Dr. Sethu Vijayakumar 1 Predictive Localization Predictive Localization Overview • Probabilistic State Estimation • Kalman Filters • Derivation of Kalman Filters • Extended Kalman Filters IAR - Dr. Sethu Vijayakumar 2 Overall Goal Overall Goal To come up with an algorithm that produces an estimate of the state by processing data from a set of explainable measurements and incorporating some kind of plant model Sensor G 1 Sensor G 2 Sensor G n KF Plant Model k k k = + z Gθ Φw Measurement Model 1 ... k k k − = + θ Fθ Bu Plant Model ˆ k θ State Estimate IAR - Dr. Sethu Vijayakumar 3 Probabilistic State Estimation Probabilistic State Estimation Recursive state estimation: Goal: Given estimate State/observation equations Initial: Prediction: Posterior: prior Refer to page 1 of derivation for details IAR - Dr. Sethu Vijayakumar 4 The Kalman Filter: Prediction The Kalman Filter: Prediction Assumes linear state/observation equations, and Gaussian noise distributions State/observation equations: State Estimation: Prediction: ( ) ( ) ( ) | 1 | 1 | 1 | 1 | 1 1 | 1 | 1 | 1 | 1 | 1 1/2 /2 | 1 ( ; , ) : 1 1 ( ; ) exp 2 2 kk kk kk kk kk T kk kk kk kk kk n kk where θ θ θ π − − − − − − − − − − − − ⎧ ⎫ ≡ − − − ⎨ ⎬ ⎩ ⎭ a P a ,P θ a P θ a P ∼ N N Final Prediction: k k-1 k k k k k k k k θ =Fθ + Φw w (w ;0,Q) z =Gθ + Ψv v (v ;0,R) ∼ ∼ N N | 1 | 1 1 kk kk k k k-1 − − − + = T T a Fa Bu P FP F + ΦQΦ =
Transcript of Predictive Localization estimate of the state
1
IAR - Dr. Sethu Vijayakumar 1
Predictive LocalizationPredictive Localization
Overview• Probabilistic State Estimation• Kalman Filters
• Derivation of Kalman Filters
• Extended Kalman Filters
IAR - Dr. Sethu Vijayakumar 2
1( , , )k k k kf −=θ θ u v
( , )k k kf=z θ w
Overall GoalOverall Goal
To come up with an algorithm that produces an estimate of the state by processing data from a set of explainable measurements and incorporating some kind of plant model
Sensor G1
Sensor G2
Sensor Gn
KF
Plant Model
k k k= +z Gθ ΦwMeasurement Model
1 ...k k k−= +θ Fθ BuPlant Model
ˆkθ
State Estimate
IAR - Dr. Sethu Vijayakumar 3
Probabilistic State EstimationProbabilistic State Estimation
Recursive state estimation:
Goal: Given estimate
State/observation equations
Initial:
Prediction:
Posterior:
prior
Refer to page 1 of derivation for details
IAR - Dr. Sethu Vijayakumar 4
The Kalman Filter: PredictionThe Kalman Filter: Prediction
Assumes linear state/observation equations, and Gaussian noise distributions
State/observation equations: State Estimation:
Prediction:
( )( ) ( )| 1
| 1 | 1 | 1 | 1
1| 1 | 1 | 1 | 1 | 11/ 2/ 2
| 1
( ; , ):
1 1( ; ) exp22 k k
k k k k k k k k
Tk k k k k k k k k kn
k k
whereθ θ
θπ −
− − − −
−− − − − −
−
⎧ ⎫≡ − − −⎨ ⎬⎩ ⎭
a P
a , P θ a P θ aP
∼ N
N
Final Prediction:
k k-1 k
k k
k k k
k k
θ = Fθ +Φww (w ;0,Q)z = Gθ +Ψvv (v ;0,R)
∼
∼
N
N
| 1
| 1 1
k k
k k k k
k -1−
− − +
= T T
a Fa Bu
P FP F +ΦQΦ
=
2
IAR - Dr. Sethu Vijayakumar 5
The Kalman Filter: UpdateThe Kalman Filter: Update
Update:
( ; , )k k k kθ θ a P∼ N
State/observation equations:
Final Prediction:
k k-1 k
k k
k k k
k k
θ = Fθ +Φww (w ;0,Q)z = Gθ +Ψvv (v ;0,R)
∼
∼
N
N
State Estimation:
IAR - Dr. Sethu Vijayakumar 6
Kalman Filter: SummaryKalman Filter: Summary
The Kalman equations are given by:
| 1
| 1
k k k
Tk k k
ν−
−
+
= −
a a W
P P WKW
=
where the “innovation” ν is:
the “innovation covariance” K is given by:
and the Kalman Gain W is given by:
| 1k k kν −= −z Ga
k|k -1T TK = GP G +ψRψ
k|k-1T -1W = P G K
| 1
| 1 | 1
k k
k k k k
k-1−
− −
= T T
a Fa
P FP F +ΦQΦ
=Prediction :
“actual observation – predicted observation”
Update :
IAR - Dr. Sethu Vijayakumar 7
Kalman Filter: Prediction / Estimation CycleKalman Filter: Prediction / Estimation Cycle
Single prediction-estimation cycle
Multiple prediction cycles
IAR - Dr. Sethu Vijayakumar 8
Stimulus Tracking Stimulus Tracking -- FormulationFormulation
State:
State Transition:
For a linear, Gaussian system:
State transition matrix:
Observation matrix:
3
IAR - Dr. Sethu Vijayakumar 9
Stimulus Tracking Stimulus Tracking -- ExamplesExamples
Tracking a target with temporary occlusion
Low deviation from linear system behavior
High deviation from linear system behavior
IAR - Dr. Sethu Vijayakumar 10
Kalman Filter with Control InputsKalman Filter with Control Inputs
| 1
| 1
k k k
Tk k k
ν−
−
+
= −
a a W
P P WKW
=
| 1k k kν −= −z Ga
k|k -1T TK = GP G +ψRψ
k|k -1T -1W = P G K
| 1
| 1 1
k k
k k k k
k -1−
− − +
= T T
a Fa Bu
P FP F +ΦQΦ
=Prediction : Update :
k k-1 k k
k k k
k k
k k
θ = Fθ +Φw + Buz = Gθ +Ψvw N(w ;0,Q)v N(v ;0,R)∼∼
Modified State Equations: Control inputs
There is very little change in the update equations with the addition of the control input
IAR - Dr. Sethu Vijayakumar 11
Implementing Kalman Filtering: Algorithm Implementing Kalman Filtering: Algorithm
new control uk ?
| 1
| 1 1
k k
k k k
k-1−
− −
=
a aP P
=
| 1
| 1 1
k k
k k k k
k -1−
− − +
= T T
a Fa Bu
P FP F +ΦQΦ
=
no yes
k = k +1
delay
new observation zk ?
| 1
| 1
k k k
k k k
−
−=
a aP P=
| 1
k|k -1
k|k -1
k k k kν −= −
T T
T -1
K = GP G +ψRψ
W = P G Kz Ga
| 1
| 1
k k k k
Tk k k
ν−
−
+
= −
a a W
P P WKW
=
no yes
Input to plant, e.g. steering wheel angles
Data from sensors enter here, e.g., compass
Estimate is last prediction
Prediction is last estimate
IAR - Dr. Sethu Vijayakumar 12
Extended (NonExtended (Non--linear) Kalman Filterlinear) Kalman Filter
Linearize state equations:
Linearize obs. equations:
Filter prediction-update:
