discrete kalman filter (dkf)
TRANSCRIPT
Contents
1.1 Definitions . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 2
1.2 Notations . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 2
1.3 Discrete Kalman Filter . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 2
1.4 Discrete Kalman Filter with Forgetting Factor λ . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 2
1.5 Discrete Kalman Filter with Varying Forgetting Factor λ . . . . . . . . . . . . . . . . . . . . . . . . . . . 3
1.6 Example . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 3
1.7 MATLAB Codes . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 12
1.8 References . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 12
1.9 Contacts . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 12
1
1.1. DEFINITIONS Discrete Kalman Filter
1.1 Definitions
• x : process model states
• φ : state-transition matrix
• H : measurement matrix
• z : measurement matrix
• x̂−0 : initial conditions of states
• P−o : initial error covariance matrix
• R : variance of measurement error matrix
• Q : variance of process model noise
• λ : forgetting factor 0 < λ < 2
• λ∗ : initial forgetting factor
1.2 Notations
•ˆ: estimate
• x̂−k : a prior estimate of xk
• x̂+k : a posterior estimate of xk
1.3 Discrete Kalman Filter
1. Guess initial values of P−0 and x̂−02. Calculate the gain :
• Kk = P−k HT(HP−k H
T +R)−1
3. Update estimate :
• x̂+k = x̂−k +Kk
(zk −Hx̂−k
)4. Update error :
• P+k = (I −KkH)P−k
5. Project ahead :
• x̂−k+1 = φx̂+k
• P−k+1 = φP+k φ
T +Q
• P−k+1 =P−
k+1+P−T
k+1
2
1.4 Discrete Kalman Filter with Forgetting Factor λ
1. Guess initial values of P−0 and x̂−02. Calculate the gain :
• Kk = P−k HT(HP−k H
T +Rλ)−1
3. Update estimate :
• x̂+k = x̂−k +Kk
(zk −Hx̂−k
)4. Update error :
• P+k = (I −KkH)
P−k
λ
5. Project ahead :
• x̂−k+1 = φx̂+k
• P−k+1 = φP+k φ
T +Q
• P−k+1 =P−
k+1+P−T
k+1
2
Mohamed Mohamed El-Sayed Atyya Page 2 of 12
1.5. DISCRETE KALMAN FILTER WITH VARYING FORGETTING FACTOR λ Discrete Kalman Filter
1.5 Discrete Kalman Filter with Varying Forgetting Factor λ
1. Guess initial values of P−0 , x̂−0 and λ∗
2. Calculate the gain :
• Kk = P−k HT(HP−k H
T +Rλk)−1
3. Update estimate :
• x̂+k = x̂−k +Kk
(zk −Hx̂−k
)4. Update error :
• P+k = (I −KkH)
P−k
λk
5. Project ahead :
• x̂−k+1 = φx̂+k
• P−k+1 = φP+k φ
T +Q
• P−k+1 =P−
k+1+P−T
k+1
2
6. Forgetting factor λ:
• ε =(zk −Hx̂+
k
)• λk+1 = 1−
(1−x̂+T
k Kk
)ε2
σ2(ε)µ(ε)
• λk+1 =
> 0.95 λk+1 = 0.95< 0.3 λk+1 = 0.3else λk+1 = λk+1
1.6 Example
• φ = 1
• H = 1
• R = 100
• Q = 1
• x̂−0 = 1
• P−0 = 0
• ∆t = 1.0256
• x(1) = 1
• x(k + 1) = φx(k) + ∆t+ normrnd(0,√Q)
• z(k) = Hx(k) + normrnd(0,√R)
• λ = 0.9
• λ∗ = 0.9
Mohamed Mohamed El-Sayed Atyya Page 3 of 12
1.6. EXAMPLE Discrete Kalman Filter
Results
Discrete Kalman Filter
Mohamed Mohamed El-Sayed Atyya Page 4 of 12
1.6. EXAMPLE Discrete Kalman Filter
Mohamed Mohamed El-Sayed Atyya Page 5 of 12
1.6. EXAMPLE Discrete Kalman Filter
Discrete Kalman Filter with Forgetting Factor λ
Mohamed Mohamed El-Sayed Atyya Page 6 of 12
1.6. EXAMPLE Discrete Kalman Filter
Mohamed Mohamed El-Sayed Atyya Page 7 of 12
1.6. EXAMPLE Discrete Kalman Filter
Mohamed Mohamed El-Sayed Atyya Page 8 of 12
1.6. EXAMPLE Discrete Kalman Filter
Discrete Kalman Filter with Varying Forgetting Factor λ
Mohamed Mohamed El-Sayed Atyya Page 9 of 12
1.6. EXAMPLE Discrete Kalman Filter
Mohamed Mohamed El-Sayed Atyya Page 10 of 12
1.6. EXAMPLE Discrete Kalman Filter
Mohamed Mohamed El-Sayed Atyya Page 11 of 12
1.7. MATLAB CODES Discrete Kalman Filter
Comments
From results we see that the error% have the data,
DKF DKF with λ DKF with varying λµ 38.3429 26.5472 22.7235σ 20.224 21.2737 17.5789
It’s seems that DKF with varying λ has the min. µ and σ
1.7 MATLAB Codes
1.3 http://goo.gl/s8KB0e
1.4 http://goo.gl/NiVVE0
1.5 http://goo.gl/KCCsGF
1.8 References
1. Robert Grover Brown and Patrick Y. C. Hwang, Introduction to Random Signals and Applied Kalman Filtering
1.9 Contacts
Mohamed Mohamed El-Sayed Atyya Page 12 of 12