Optimal State Estimation , Sensor Fusion Study - Ch8. The Continuous-Time Kalman Filter

1. Sensor Fusion Study AI Robotics (2020)
Haegu Lee
CHAPTER8
The continuous-time Kalman filter

2. Sensor Fusion Study AI Robotics (2020)
Haegu Lee
8.1 Discrete-Time and Continuous-Time White Noise
Discrete time system
Covariance of state Covariance of state
Continuous time system
t = KT
(k : step number T : sample time)
when state transition matrix = Identity matrix
8.1.1 process noise
(covariance of state linearly increase)

3. Sensor Fusion Study AI Robotics (2020)
Haegu Lee
Covariance of state
Continuous time systemDiscrete time system
Covariance of state
t = KT
8.1 Discrete-Time and Continuous-Time White Noise
8.1.1 process noise
discrete time white noise covariance Q with sample time T
= Continuous time white noise covariance Qc = Q/T

4. Sensor Fusion Study AI Robotics (2020)
Haegu Lee
Continuous time measurementDiscrete time measurement
estimation error covariance estimation error covariance
8.1 Discrete-Time and Continuous-Time White Noise
8.1.2 Measurement noise
(posteriori estimation-error covariance)
(measurement of constant x)
(t = kT)
error covariance at time t is independent of sample time T
if (Rc is constant)
effect of white m.noise in d.t and c.t are the same

5. Sensor Fusion Study AI Robotics (2020)
Haegu Lee
8.1 Discrete-Time and Continuous-Time White Noise
Continuous time systemDiscrete time system
only valid if inv(A) exist
Integrate discretized
equation to simulate
Continuous time system
* Integrating method, check section 1.5
8.1.3 Discretized simulation of noisy continuous time system
to implement a discrete time state estimator

6. Sensor Fusion Study AI Robotics (2020)
Haegu Lee
8.2 Derivation of the continuous-time Kalman filter
Continuous time system Discrete time system
discretize with sample time T

7. Sensor Fusion Study AI Robotics (2020)
Haegu Lee
Discrete time system
• Kalman gain
• estimator error covariance
8.2 Derivation of the continuous-time Kalman filter
(Ricatti differential equation)

8. Sensor Fusion Study AI Robotics (2020)
Haegu Lee
• Differential Ricatti equation
Use it compute estimation error covariance
P = nxn matrix
integration of P(nxn matrix) requires n^2
but p = estimator error covariance = symmetric
i.e. integration of p requires n(n+1)/2
8.2 Derivation of the continuous-time Kalman filter
• estimate value measurement update
change to continuous

9. Sensor Fusion Study AI Robotics (2020)
Haegu Lee
The continuous kalman filter process summary
System dynamics and measurement equation Continuous time kalman filter equation
8.2 Derivation of the continuous-time Kalman filter

10. Sensor Fusion Study AI Robotics (2020)
Haegu Lee
8.3 Alternative solutions to the Riccati equation
Ricatti equation
Problem
1. Computationlly expensive to integrate
2. losing positive definiteness due to numerical problems
Other method
1. Transition matrix approach
2. The Chandrasekhar algorithm
3. Square root filtering
* Chandrasekhar, nobel prize winning astrophysicist
P = nxn matrix
integration of P(nxn matrix) requires n^2
but p = estimator error covariance = symmetric
i.e. integration of p requires n(n+1)/2

11. Sensor Fusion Study AI Robotics (2020)
Haegu Lee
1. Transition matrix approach
• Factorization
8.3 Alternative solutions to the Riccati equation
( nxn matrix )
Need to check if Factorization is valid
Assume factorization is valid

12. Sensor Fusion Study AI Robotics (2020)
Haegu Lee
1. Transition matrix approach
• Factorization check
8.3 Alternative solutions to the Riccati equation
Initial condition on P, and A, Q, C, R constant
(LTI system)
J is constant
• error covariance

13. Sensor Fusion Study AI Robotics (2020)
Haegu Lee
1. Transition matrix approach
8.3 Alternative solutions to the Riccati equation
• error covariance
Faster than integrating Riccati eq,
No need to consider step size
P(t+T) propagate from P(t)

14. Sensor Fusion Study AI Robotics (2020)
Haegu Lee
output of system -> input to filter
consider zero input kalman filter
2. The Chandrasekhar algorithm
8.3 Alternative solutions to the Riccati equation
• differential equation for state estimate
Assume system is time invariant, K is constant
only when A,C,R,Q is constant
state transition matrix
differentiate to get this form
• error covariance
symmetric P

15. Sensor Fusion Study AI Robotics (2020)
Haegu Lee
2. The Chandrasekhar algorithm
8.3 Alternative solutions to the Riccati equation
• error covariance
symmetric P
nxn matrix
rank of a < n
all eigenvalue is real
number of positive eigenvalue = b
number of negative eigenvalue = (a-c)
is n x b matrix
is n x (a-c) matrix

16. Sensor Fusion Study AI Robotics (2020)
Haegu Lee
2. The Chandrasekhar algorithm
8.3 Alternative solutions to the Riccati equation
• Factorization
nxn matrix
rank of a < n
all eigenvalue is real
number of positive eigenvalue = b
number of negative eigenvalue = (a-c)
is n x b matrix
is n x (a-c) matrix
S comprise the eigenvector of diff(P)
D1, D2 is Jordan form
bxb , (a-c)x(a-c)

17. Sensor Fusion Study AI Robotics (2020)
Haegu Lee
3. Square root filter
8.3 Alternative solutions to the Riccati equation
• Problem
1. Numerical computation sometimes make P nonsymmetric
2. Computer :Okay but How about Embedded system?
(8bit , 16bit..? now 32bit though)
• Purpose
1. To increase Numerical percision
• Basic idea
Finding S matrix that P = S*S^T (S : a square root of P)
some books use P = S^2, P=S^T*S (S : a square root of P)
P = S*S^Twe use this one
For square roof of P, S is not always only one
(ex, square roof of 4 = -2 / +2)
get estimate error covariance by integrating S ( not P)
Computationally expensive
but give higher percision

18. Sensor Fusion Study AI Robotics (2020)
Haegu Lee
3. Square root filter
8.3 Alternative solutions to the Riccati equation
• error covariance
-1 -
-T
-
Upper triangular
Lower triangular
.
. T
ingerate to get Kalman gain
cf.

19. Sensor Fusion Study AI Robotics (2020)
Haegu Lee
8.4 Generalization of the continuous time filter
• previous assumtion
1. process and measurement noise is uncorrelated
2. noises are white
1. process and measurement noise is correlated
2. noises are colored
• discussion

20. Sensor Fusion Study AI Robotics (2020)
Haegu Lee
1. Correlated process and measurement noise
8.4 Generalization of the continuous time filter
known input
new process noise
Uncorrelated
• new process noise & measurement noise realation
• Covariance of new process noise
• system dynamics and measurement

21. Sensor Fusion Study AI Robotics (2020)
Haegu Lee
1. Correlated process and measurement noise
8.4 Generalization of the continuous time filter
Only modify Kalman gain
• General form of the continuous Kalman filter
if M = 0, this equation become standard
continuous time filter
i.e) this is the general form of the continutous
time Kalman filter.

22. Sensor Fusion Study AI Robotics (2020)
Haegu Lee
2. Colored measurement noise
8.4 Generalization of the continuous time filter
• system dynamics and measurement • process noise & new measurement noise relation
• Covariance of new process noise

23. Sensor Fusion Study AI Robotics (2020)
Haegu Lee
2. Colored measurement noise
8.4 Generalization of the continuous time filter
• new measurement noise
Already noisy
• Avoid using noisy Y
• Define a new signal
Integrating diff(z) to get z
get differentiated K to compute diff(z)
-> much easier with smooth y
rather than very noisy measurement y

24. Sensor Fusion Study AI Robotics (2020)
Haegu Lee
1. The algebraic Riccati equation
8.5 The steady state continuous-time Kalman filter
(Riccati equation)
P reach a steady-state value
estimation error covariance goes to Zero
• Continuous algebraic Riccati equation(C.ARE)
ARE solution not always exist
existing solution may not result in a stable in K.F
• Controllability on the imaginary axis
( R >0, Q>0 )
• steady state Kalman filter
CARE solution P is stabilizing if it result in stable steady-state
= eigenvalue of have negative parts

25. Sensor Fusion Study AI Robotics (2020)
Haegu Lee
1. The algebraic Riccati equation
8.5 The steady state continuous-time Kalman filter
• Controllability on the imaginary axis
( R >0, Q>0 )
• steady state Kalman filter
CARE solution P is stabilizing if it result in stable steady-state
= eigenvalue of have negative parts

26. Sensor Fusion Study AI Robotics (2020)
Haegu Lee
1. The Wiener filter is a Kalman filter
8.5 The steady state continuous-time Kalman filter
• steady state continuous time Kalman filter
same form as the Wiener filter
-> the Wiener filter is
special case of Kalman filter
Laplace transform

27. Sensor Fusion Study AI Robotics (2020)
Haegu Lee
2. Duality
8.5 The steady state continuous-time Kalman filter
• system dynamics and measurement
• optimal estimater
minimize cost function
optimal estimation problem optimal control problem
• system
• optimal controller
minimize cost function

28. Sensor Fusion Study AI Robotics (2020)
Haegu Lee
끄읏