![Av-738
Adaptive Filter Theory
Lecture 4- Optimum Filtering
[Extended Kalman Filters]
Dr. Bilal A. Siddiqui
Air University (PAC Campus)
Spring 2018](https://image.slidesharecdn.com/av-738-aft-spr18-lecture05-optimumfilters-extkalmanwk5-180220135210/85/Av-738-Adaptive-Filters-Extended-Kalman-Filter-1-320.jpg)
Av 738-Adaptive Filters - Extended Kalman Filter
- 1. Av-738 Adaptive Filter Theory Lecture 4- Optimum Filtering [Extended Kalman Filters] Dr. Bilal A. Siddiqui Air University (PAC Campus) Spring 2018
- 2. All systems are ultimately nonlinear • All of our discussion to this point has considered linear filters for linear systems. Unfortunately, linear systems do not exist. All systems are ultimately nonlinear. • But nonlinear problems are very difficult to solve. Therefore, it is often a compromise between simplicity and performance. Famous physicist Richard Feynman: “Linear systems are important because we can solve them.” • However, many systems are close enough to linear that linear estimation approaches give satisfactory results. Eventually, we run across a system that does not behave linearly even over a small range of operation. In this case, we need to explore nonlinear estimators. • Schwartz and Stear “No linear filter is consistently better than any other linear filter…but any nonlinear filter is better than a strictly linear one.” •
- 3. Extended Kalman Filter (EKF) • The Kalman filter that we discussed in previous lecture directly applies only to linear systems. • However, a nonlinear system can be linearized, and then linear estimation techniques (such as the Kalman or H∞) can be applied. • In this lecture, we will discuss the linearized Kalman filter. This will involve finding a linear system whose states represent the deviations from a nominal tra- jectory of a nonlinear system. • We can then use the Kalman filter to estimate the deviations from the nominal trajectory, and hence obtain an estimate of the states of the nonlinear system. • We will extend the linearized Kalman filter to directly estimate the states of a nonlinear system. • This filter, called the extended Kalman filter (EKF), is undoubtedly the most widely used nonlinear state estimation technique that has been applied in the past few decades. • It is also the work horse of aerospace navigation.
- 4. Linearized Kalman Filter • Consider the following general nonlinear system model: • Here, f(.) and h(.) are nonlinear functions. • We will use Taylor series to expand these equations around a nominal control uo, nominal state xo, nominal output yo, and nominal noise values wo and vo. • These nominal values (all of which are functions of time) are based on a priori guesses of what the system trajectory might look like. • For example, if the system equations represent the dynamics of an airplane, then the nominal control, state, and output might be the planned flight trajectory. The actual flight trajectory will differ from this nominal trajectory due to mismodeling, disturbances, and other unforeseen effects. • But the actual trajectory should be close to the nominal trajectory, in which case the Taylor series linearization should be approximately correct.
- 5. Taylor Series expansion • Since wo(t)=0 and vo(t) = 0, Δw(t) = w(t) and Δ v(t) = v(t). • The inputs to the filter consist of Δy, which is the difference between the actual measurement y and the nominal measurement yo. • Δx is output from the Kalman filter. It is an estimate of the difference between the actual state x and the nominal state xo. • Assume that the control u(t) is perfectly known. Since it is determined by our control system, so there should not be any uncertainty in its value. This means that uo(t) = u(t) and Δ u(t) = 0. • The linearized Kalman filter would then be
- 6. Extended Kalman Filter • We obtained a linearized KF for estimating the states of a nonlinear system. • Derivation based on linearizing nonlin sys around a nominal state trajectory. • How do we know the nominal state trajectory? • In some cases it may not be straightforward to find the nominal trajectory. • Since the Kalman filter estimates the state of the system, we can use the Kalman filter estimate as the nominal state trajectory. • This is sort of an adhoc method. This is the idea of the extended Kalman filter (EKF), which was originally proposed by Stanley Schmidt so that the Kalman filter could be applied to nonlinear spacecraft navigation problems
- 7. Discrete-time EKF • Suppose we have the system • We perform a Taylor series expansion of the state equation around𝑥 𝑘−1 + and 𝑤 𝑘−1 = 0
- 9. Example 𝛾 𝛾
- 10. Solution (1) • First, let’s find the discrete time equivalent of the system 𝑥1 𝑘 = 𝑥1 𝑘−1 + 𝑇𝑥2 𝑘−1 + 𝑇𝑤1 𝑘−1 𝑥2 𝑘 = 𝑥2 𝑘−1 + 𝑇𝜌0 𝑒 − 𝑥1 𝑘−1 𝑥2 𝑘−1 2 2𝛾𝑥3 𝑘 − 𝑇𝑔 + 𝑇𝑤2 𝑘−1 𝑥3 𝑘 = 𝑥3 𝑘−1 + 𝑇𝑤3 𝑘−1 𝑦 𝑘 = 𝑥1 𝑘 + 𝑣 𝑘
- 11. Step a 𝐹𝑘−1 = 1 𝑇 0 𝐴1 𝐴2 𝐴3 0 0 1 Where 𝐴1 = − 𝑥2 𝑘−1 2 2𝛾𝑥3 𝑘 + 𝑇𝜌0 𝑒 − 𝑥1 𝑘−1 + 𝑥2 𝑘−1 +2 2𝛾𝑥3 𝑘 + , 𝐴2 = 1 − 𝑥1 𝑘−1 + 𝑥2 𝑘−1 + 𝛾𝑥3 𝑘−1 + 𝑇𝜌0 𝑒 − 𝑥1 𝑘−1 + 𝑥2 𝑘−1 +2 2𝛾𝑥3 𝑘−1 + , 𝐴3 = 𝑥2 𝑘−1 2 𝛾𝑥3 𝑘−1 +2 𝑇𝜌0 𝑒 − 𝑥1 𝑘−1 + 𝑥2 𝑘−1 +2 2𝛾𝑥3 𝑘−1 + 𝐿 𝑘−1 = 𝑇 0 0 0 𝑇 0 0 0 𝑇
- 12. Step b-d Since 𝑦 𝑘 = 𝑥1 𝑘 + 𝑣 𝑘 𝐻 𝑘 = 1 0 0 𝑀 𝑘 = 1
- 13. Take Home Pre-Mid Exam (submit by 8 am Thursday) – Question 1 • Find altitude, velocity, and ballistic coefficient reciprocal estimation-error magnitudes of a falling body averaged over 100 simulations for the previous example. • Submit by email to airbilal(at)gmail.com by 9 pm tonight. Late submisisons and copied submissions will not be entertained. • Compare with a linear Kalman filter in which F,L, H and M matrices are constant, linearized at some midlevel altitude. • Now change the situation. We change the measurement system so that it does not measure the altitude of the falling body, but instead measures the range to the measuring device. The measuring device is Iocated at an altitude “a” and at a horizontal distance “M” from the body's vertical line of fall. The measurement equation is therefore given by • Repeat the question.
- 14. Parameter Estimation • Kalman filters have been successfully used in system identification / parameter estimation applications. • We can consider the unknown parameter to be a constant (or very slow varying) and then estimate it as a ‘state’. • Suppose we have the following system
- 15. Parameter Estimation - continued
- 16. Solution • First, let’s find the discrete time equivalent of the system 𝑥1 𝑘 = 𝑥1 𝑘−1 + 𝑇𝑥2 𝑘−1 𝑥2 𝑘 = 𝑥2 𝑘−1 + 𝑇(𝑥3 𝑘−1 𝑥1 𝑘−1 + 𝑏𝑥2 𝑘−1 − 𝑥3 𝑘−1 𝑤 𝑘−1) 𝑥3 𝑘 = 𝑥3 𝑘−1 + 𝑇𝑤 𝑝 𝑘−1
- 17. Steps of EKF 𝐹𝑘−1 = 1 𝑇 0 𝑇𝑥3 𝑘−1 + 1 + 𝑏𝑇 𝑇𝑥1 𝑘−1 + 0 0 1 , 𝐿 𝑘−1 = 0 −𝑇𝑥3 𝑘−1 + 0 𝐻 𝑘 = 1 0 0 0 1 0 𝑀 𝑘 = 1
- 18. Take Home Pre-Mid Exam (submit by 8 am Thursday) – Question 2 • Solve the above example on Matlab. • The true system parameters are 𝜔 𝑛= 2 and 𝜁= 0.1. Suppose that we begin by estimating 𝜔 𝑛 2 as -8 with an initial estimation variance of 20. • We set the variance of the artificial noise 𝜔 𝑝 = 0.1. • In how many samples were you able to converge? • Also, set different values of 𝜔 𝑝 and see its effect on parameter estimates. • Solutions will be posted online in the code folder after deadline. • It is very easy to figure out if you are merely copying someone else’s code. So, try to do your own work. It’s all about learning.