Published on

  • Be the first to comment

  • Be the first to like this

No Downloads
Total views
On SlideShare
From Embeds
Number of Embeds
Embeds 0
No embeds

No notes for slide


  1. 1. System Identification Seminar on Kalman Filtering By: Mahsa Rezaei (909238) School of Electrical and Computer Engineering February 2012 1
  2. 2. Outline:  Motivation  Historical Prospective  What is a Kalman filter?  Applications  Advantages  The problem  How does Kalman filter work?  Estimation in linear systems  What criteria should our estimator satisfy?  Preparation before going to Algorithm  Standard Kalman filter Algorithm  Example on Kalman filter for linear systems (Vehicle Navigation)  Shift to nonlinear systems  Extended Kalman filter Algorithm  Example on Extended Kalman filter for Nonlinear Systems  References 2
  3. 3. Motivation: filtering is desirable in many situations in engineering and embedded systems. For example, radio communication signals are corrupted with noise. A good filtering algorithm can remove the noise from electromagnetic signals while retaining the useful information. Another example is power supply voltages. Uninterruptible power supplies are devices that filter line voltages in order to smooth out undesirable fluctuations that might otherwise shorten the lifespan of electrical devices such as computers and printers. 3
  4. 4. Historical Prospective: In 1960 and 1961 Rudolf Emil Kalman published his papers on a recursive predictive filter that is based on the use of state space techniques and recursive algorithms and therewith he revolutionized the field of estimation. Like many new technologies, the Kalman filter was developed to solve a specific problem, in this case, spacecraft navigation for the Apollo space program. Since that time the so-called Kalman filter has been the subject of extensive research and applications. Sometimes the filter is referred to as the Kalman-Bucy filter because of Richard Bucy‟s early work on the topic, conducted jointly with Kalman. 4
  5. 5. What is a Kalman filter? The Kalman filter is a set of mathematical equations that provides an efficient computational (recursive) means to estimate the state of a dynamic system, in a way that minimizes the mean of the squared error. The filter is very powerful in the sense that it supports estimations of past, present, and even future states, and it can do so even when the precise nature of the modeled system is unknown. 5
  6. 6. Applications: The applications of a Kalman filter are numerous:  Tracking objects (e.g., missiles, faces, heads, hands)  All forms of navigation (aerospace, land, and marine),  Economics  Fuzzy logic  Neural network training  The detection of underground radioactivity,  Many computer vision applications  Many more The principal uses of Kalman filtering have been in „Modern‟ control systems, in the tracking and navigation of all sorts of vehicles, and predictive design of estimation and control systems. 6
  7. 7. Advantages: 1.The Kalman filter is implementable in the form of an algorithm for a digital computer. This implementation may be slower, but it is capable of much greater accuracy than had been achieved with analog filters. 2.The Kalman filter does not require that the deterministic dynamics or the random processes have stationary properties, and many applications of importance include non-stationary stochastic processes. 3.The Kalman filter is compatible with the state-space formulation of optimal controllers for dynamic systems . 4.The Kalman filter requires less additional mathematical preparation to learn and use than other filters. 7
  8. 8. The problem: Measuring Devices Estimator Measurement Error Sources System State (desired but not known) External Controls Observed Measurements Optimal Estimate of System State System Error Sources System Black Box • System state cannot be measured directly • Need to estimate “optimally” from measurements 8
  9. 9. How does Kalman filter work? The Kalman filter estimates the state of a dynamic system. This dynamic system can be disturbed by some noise, mostly assumed as white noise. To improve the estimated state, the Kalman filter uses measurements that are related to the state but disturbed as well. Thus the Kalman filter consists of two steps: 1.The prediction 2.The correction 9
  10. 10. In the first step the state is predicted with the dynamic model. In the second step it is corrected with the observation model, so that the error covariance of the estimator is minimized. In this sense it is an optimal estimator. This procedure is repeated for each time step with the state of the previous time step as initial value. Therefore the kalman filter is called a recursive filter. 10
  11. 11. Estimation in linear systems: Many physical processes, such as a vehicle driving along a road, a satellite orbiting the earth, a motor shaft driven by winding currents, or a sinusoidal radio- frequency carrier signal, can be approximated as linear systems. A linear system is simply a process that can be described by the following two equations: 𝒙 𝒌 = 𝑨 𝒌−𝟏 𝒙 𝒌−𝟏 + 𝑩 𝒌−𝟏 𝒖 𝒌−𝟏 + 𝑳 𝒌−𝟏 𝒘 𝒌−𝟏 𝒚 𝒌 = 𝑯 𝒌 𝒙 𝒌 + 𝑴 𝒌 𝒗 𝒌 𝒙 𝒏×𝟏: State vector 𝒖 𝒓×𝟏: input vector 𝒘 𝒑×𝟏: Process(state) noise 𝒚 𝒎×𝟏: measurement(output) vector 𝒗 𝒈×𝟏: measurement error 𝒌 = 𝟎, 𝟏, 𝟐, … :Discrete time index 𝑨 𝒏×𝒏: transition matrix which relates the state at time k to the one at k-1 𝑩 𝒏×𝒓: relates the optional control input u to the state x 𝑯 𝒎×𝒏: relates the measurement to the state 11
  12. 12. What criteria should our estimator satisfy? We want an estimator that gives an accurate estimate of the true state even though we cannot directly measure it. What criteria should our estimator satisfy? Two obvious requirements come to mind. First, we want the average value of our state estimate to be equal to the average value of the true state. That is, we don‟t want our estimate to be biased one way or another. Second, we want a state estimate that varies from the true state as little as possible. That is, not only do we want the average of the state estimate to be equal to the average of the true state, but we also want an estimator that results in the smallest possible variation of the state estimate. 12
  13. 13. Preparation before going to algorithm: 1.Definition: 𝒙−: a priori state, the predicted value before the update 𝒙+: a posteriori state, the corrected value after the update 2.Expected value of a random variable x is: 𝑬 𝒙 = 𝒙 𝑷 𝒙 𝒅𝒙 ≈ 𝒙 = 𝟏 𝑵 𝒙𝒊 𝑵 𝒊=𝟏 Expected value of a vector X is by component: 𝑬 𝑿 = 𝑿 = 𝑿 𝟏, 𝑿 𝟐, … , 𝑿 𝒏 𝑻 13
  14. 14. 3.For the expressed criteria to be satisfied, certain condition about the noise- that affects our system- should be assumed: 𝑬 𝒘 𝒌 = 𝟎 𝑬 𝒗 𝒌 = 𝟎 𝑬 𝒘 𝒌 𝒗 𝒌 = 𝟎 𝒘 𝒌~(𝟎, 𝑸 𝒌) 𝑬 𝒘 𝒌 𝒘𝒋 𝑻 = 𝟎 𝒇𝒐𝒓 𝒌 ≠ 𝒋 𝑬 𝒗 𝒌 𝒗𝒋 𝑻 = 𝟎 𝒇𝒐𝒓 𝒌 ≠ 𝒋 𝒗 𝒌~(𝟎, 𝑹 𝒌) 4.Definition: 𝑸 𝒌= 𝑬 𝒘 𝒌 𝒘 𝒌 𝑻 : Covariance of w 𝑹 𝒌 = 𝑬(𝒗 𝒌 𝒗 𝒌 𝑻) : Covariance of v 14
  15. 15. Standard Kalman filter algorithm: Step 0: Initialization of the filter at k=0. 𝒙 𝟎 + = 𝑬 𝒙 𝟎 𝑷 𝟎 + = 𝑬 𝒙 𝟎 − 𝒙 𝟎 + 𝒙 𝟎 − 𝒙 𝟎 + 𝑻 Step 1: for k=1,2,… follow the following procedure. a) 𝒙 𝒌 − = 𝑨 𝒌−𝟏 𝒙 𝒌−𝟏 + + 𝑩 𝒌−𝟏 𝒖 𝒌−𝟏 𝑷 𝒌 − = 𝑨 𝒌−𝟏 𝑷 𝒌−𝟏 + 𝑨 𝒌−𝟏 𝑻 + 𝑳 𝒌−𝟏 𝑸 𝒌−𝟏 𝑳 𝒌−𝟏 𝑻 b) 𝑲 𝒌 = 𝑷 𝒌 − 𝑯 𝒌 𝑻 𝑯 𝒌 𝑷 𝒌 − 𝑯 𝒌 𝑻 + 𝑴 𝒌 𝑹 𝒌 𝑴 𝒌 𝑻 −𝟏 c) 𝒙 𝒌 + = 𝒙 𝒌 − + 𝑲 𝒌 𝒚 𝒌 − 𝑯 𝒌 𝒙 𝒌 − 𝑷 𝒌 + = (𝑰 − 𝑲 𝒌 𝑯 𝒌)𝑷 𝒌 − 15
  16. 16. Example on standard Kalman filter for linear systems (Vehicle Navigation): we want to model a vehicle going in a straight line. We can say that the state consists of the vehicle position p and velocity v. The input u is the commanded acceleration and the output y is the measured position. we are able to change the acceleration and measure the position every T seconds. In this case, elementary laws of physics say that the position p and velocity v will be governed by the following equations: 𝒑 𝒌 = 𝒑 𝒌−𝟏 + 𝑻𝒗 𝒌−𝟏 + 𝟏 𝟐 𝑻 𝟐 𝒖 𝒌−𝟏 + 𝒑 𝒌−𝟏 𝒗 𝒌 = 𝒗 𝒌−𝟏 + 𝑻𝒖 𝒌−𝟏 + 𝒗 𝒌 Where 𝑝 𝑘 and 𝑣 𝑘 are position and velocity noise respectively. Now we can define a state vector x that consists of position and velocity: 𝒙 𝒌 = 𝒑 𝒌 𝒗 𝒌 16
  17. 17. Finally, knowing that the measured output is equal to the position, we can write our linear system equations as follows: 𝒙 𝒌 = 𝟏 𝑻 𝟎 𝟏 𝒙 𝒌−𝟏 + 𝑻 𝟐 𝟐 𝑻 𝒖 𝒌 + 𝒘 𝒌 𝒚 𝒌 = 𝟏 𝟎 𝒙 𝒌 + 𝒛 𝒌 𝑧 𝑘 is the measurement noise due to such things as instrumentation errors. If we want to control the vehicle with some sort of feedback system, we need an accurate estimate of the position p and the velocity v. In other words, we need a way to estimate the state x. This is where the Kalman filter comes in. 17
  18. 18. The position is measured with an error of 10 feet (one standard deviation). The commanded acceleration is a constant 1 foot/sec2. The acceleration noise is 0.2 feet/sec2 (one standard deviation). The position is measured 10 times per second (T = 0.1). Since T = 0.1, the linear model that represents our system can be derived from the system model presented earlier as follows: 𝒙 𝒌 = 𝟏 𝟎. 𝟏 𝟎 𝟏 𝒙 𝒌−𝟏 + 𝟎. 𝟎𝟎𝟓 𝟎. 𝟏 𝒖 𝒌 + 𝒘 𝒌 𝒚 𝒌 = 𝟏 𝟎 𝒙 𝒌 + 𝒛 𝒌 Because the standard deviation of the measurement noise is 10 feet, the 𝑅 𝑘 matrix is simply equal to 100. Now we need to derive the 𝑄 𝑘 matrix. 18
  19. 19. Since the position is proportional to 0.005 times the acceleration, and the acceleration noise is 0.2 feet/sec2, the variance of the position noise is (0.005)2× (0.2)2= 10−6. Similarly, since the velocity is proportional to 0.1 times the acceleration, the variance of the velocity noise is (0.1)2× (0.2)2= 4 × 10−4. Finally, the covariance of the position noise and velocity noise is equal to the standard deviation of the position noise times the standard deviation of the velocity noise, which can be calculated as 0.005 × 0.2 × 0.1 × 0.2 = 2 × 10−5. We can combine all of these calculations to obtain the following matrix for 𝑄 𝑘 : 𝑸 𝒌 = 𝑬 𝒙𝒙 𝑻 = 𝑬 𝒑 𝒗 𝒑 𝒗 = 𝑬 𝒑 𝟐 𝒑𝒗 𝒑𝒗 𝒗 𝟐 = 𝟏𝟎−𝟔 𝟐 × 𝟏𝟎−𝟓 𝟐 × 𝟏𝟎−𝟓 𝟒 × 𝟏𝟎−𝟒 I simulated the Kalman filter for this problem using Matlab. The results are shown in the accompanying figures. 19
  20. 20. Figure 1 shows the true position of the vehicle, the measured position, and the estimated position. 20
  21. 21. The two smooth curves are the true position and the estimated position, and they are almost too close to distinguish from one another. The noisy-looking curve is the measured position. For better comparison, part of the figure is magnified here: 21
  22. 22. Figure 2 shows the error between the true position and the measured position, and the error between the true position and the Kalman filter‟s estimated position. 22
  23. 23. Figure 3 shows the True and Estimated Velocity 23
  24. 24. Figure 4 shows the error between the true velocity and the Kalman filter‟s estimated velocity. 24
  25. 25. Shift to Nonlinear Systems: We have discussed state estimation for linear systems. But what if we want to estimate the states of a nonlinear system? As a matter of fact, almost all real engineering processes are nonlinear. Some can be approximated by linear systems but some cannot. This was recognized early in the history of Kalman filters and led to the development of the “extended Kalman filter” which is simply an extension of linear Kalman filter theory to nonlinear systems. A Nonlinear system is simply a process that can be described by the following two equations: 𝒙 𝒌 = 𝒇 𝒌−𝟏 𝒙 𝒌−𝟏, 𝒖 𝒌−𝟏 , 𝒘 𝒌−𝟏 𝒚 𝒌 = 𝒉 𝒌 𝒙 𝒌, 𝒖 𝒌 , 𝒗 𝒌 f : System function h : Output function 25
  26. 26. Extended Kalman filter Algorithm: Step 0: Initialization of the filter at k=0 𝒙 𝟎 + = 𝑬 𝒙 𝟎 𝑷 𝟎 + = 𝑬 𝒙 𝟎 − 𝒙 𝟎 + 𝒙 𝟎 − 𝒙 𝟎 + 𝑻 Step 1: for k=1,2,… follow the following procedure. a) 𝑭 𝒌−𝟏 = 𝝏𝒇 𝒌−𝟏 𝝏𝒙 𝒙 𝒌−𝟏 + 𝑳 𝒌−𝟏 = 𝝏𝒇 𝒌−𝟏 𝝏𝒘 𝒙 𝒌−𝟏 + b) 𝒙 𝒌 − = 𝒇 𝒌−𝟏 𝒙 𝒌−𝟏 + , 𝒖 𝒌−𝟏, 𝟎 26
  27. 27. 27 𝑷 𝒌 − = 𝑭 𝒌−𝟏 𝑷 𝒌−𝟏 + 𝑭 𝒌−𝟏 𝑻 + 𝑳 𝒌−𝟏 𝑸 𝒌−𝟏 𝑳 𝒌−𝟏 𝑻 c) 𝑯 𝒌 = 𝝏𝒉 𝒌 𝝏𝒙 𝒙 𝒌 − 𝑴 𝒌 = 𝝏𝒉 𝒌 𝝏𝒗 𝒙 𝒌 − d) 𝑲 𝒌 = 𝑷 𝒌 − 𝑯 𝒌 𝑻 𝑯 𝒌 𝑷 𝒌 − 𝑯 𝒌 𝑻 + 𝑴 𝒌 𝑹 𝒌 𝑴 𝒌 𝑻 −𝟏 e) 𝒙 𝒌 + = 𝒙 𝒌 − + 𝑲 𝒌 𝒚 𝒌 − 𝒉 𝒌 𝒙 𝒌 − , 𝟎 𝑷 𝒌 + = (𝑰 − 𝑲 𝒌 𝑯 𝒌)𝑷 𝒌 −
  28. 28. 28 Example on Extended Kalman filter for Nonlinear systems: Consider a second order system as below: 𝒚 𝒖 = 𝝎 𝒏 𝟐 𝒔 𝟐 + 𝟐𝜻𝝎 𝒏 𝒔 + 𝝎 𝒏 𝟐 Define 𝑏 = 2𝜁𝜔 𝑛 We want to estimate 𝝎 𝒏 𝟐. In terms of differential equations: 𝒚 + 𝒃𝒚 + 𝝎 𝒏 𝟐 𝒚 = 𝝎 𝒏 𝟐 𝒖 And so we define the states as: 𝒙 𝟏 = 𝒚 𝒙 𝟏 = 𝒙 𝟐 𝒙 𝟐 = 𝒚 𝒙 𝟐 = −𝒃𝒙 𝟐 − 𝒙 𝟏 𝒙 𝟑 + 𝒙 𝟑 𝒖 𝒙 𝟑 = 𝝎 𝒏 𝟐 𝒙 𝟑 = 𝟎
  29. 29. 29 Assume that: 𝝎 𝒏 = 𝟏 : true value 𝜻 = 𝟎. 𝟏 : zeta 𝑸 = 𝟏𝟎𝟎𝟎 𝟎 𝟎 𝟎. 𝟏 : Covariance of process noise 𝑹 = 𝟏𝟎 𝟎 𝟎 𝟏𝟎 : Covariance of measurement noise The simulation results are enclosed in the accompanying figures.
  30. 30. 30 This figure shows the true and estimated value of 𝜔 𝑛 2
  31. 31. 31 Variance of estimation error is implemented in this figure
  32. 32. 32 References: 1. J. S. Meditch. Stochastic optimal linear estimation and control 2. Simon J. Julier and Jeffrey K. Uhlmann. A new extension of the Kalman filter to nonlinear systems. 3. Mohinder S. Grewal and Angus P. Andrews. Kalman filtering: Theory and Practice using Matlab 4. Rachel Kleinbauer . Kalman filtering Implementation with Matlab 5. Greg Welch and Gary Bishop. An introduction to the Kalman filter
  33. 33. 33 Thank You