Mathematical description of Kalman filter & Matlab examples

1. Kalman Filters basics Tim Mazumdar BR Karthikeyan

2. The immortal one Hungarian-born U.S. scientist and professor Rudolf Emil Kalman (born 1930) is widely regarded as the creator of modern control theory and system theory. His research reshaped the field of control engineering. His most widely known accomplishment is his development of the Kalman filter, a mathematical method now widely used in of EE, ME and Communication. He was born in Budapest, Hungary, on May 19, 1930, the son of an electrical engineer. He immigrated to the United States in 1943, and studied EE at MIT. Kalman received his BSEE, MIT 1953 and his MSEE in 1954,. He got his PhD(1957) under John R. Ragazinni at Columbia. The original paper on Kalman filtering was entitled “A New Approach to Linear Filtering and Prediction Problems,” ASME—Transactions March 1960.

3. In Dedication to the man who taught K filtering Professor Uday B. Desai –respected academic, Professor at IIT, Bombay Father of Orthogonal Filters, the Desai-Weinert Smoother, the Robust Recursive Least Squares algorithm and about 100 other major innovations B Tech , IIT B- 1969-1974 PhD Johns Hopkins University – 1979. Currently Director IIT, Hyderabad

12. What made the Kalman filter so celebrated in Media Kalman’s original work was funded by Air Force Office of Scientific Research (AFOSR) as it related to space vehicles. The AFOSR sponsored the research done at RIAS by Kalman and Bucy. Kalman and Bucy's work revolutionized the field of estimation and had an enormous impact on the design and development of precise navigation systems. The Kalman filter was a major breakthrough in guidance technology. In fact the original paper is the 25 th most quoted Paper in all of EE. It was the basis of the Apollo Guidance system and it enabled the first moon landing. It was the core of a memo written by Donald Fraser in 1965 for Apollo Guidance systems “ Recursive filtering applied to system identification” and used by James Potter . It is debatable whether the moon landings were possible without the Kalman Filter. Stanley F. Schmidt had a key role in the proselytizing NASA on the K filter. Smith convinced his colleagues to study and use the K filter for the Apollo missions. He also wrote the first code. The Kalman filter solved two problems that NASA wanted to solve . DATA FUSION PROBLEM – where RADAR data is combined With inertial navigation sensor data. DATA REJECTION PROBLEM – data outliers Need to be rejected for the spacecraft to continue in its proper trajectory.

13. STRUCTURE OF THE GOVERNING KALMAN FILTER EQUATIONS STATE EQUATION STATE NOISE MEASUREMENT EQUATION MEASUREMENT NOISE

14. KALMAN FILTER IN TERMS OF PROBABILITY, STATISTICS, AND RANDOM PROCESS UP ARROW: TIME INDEPENDENT; DOWN ARROW: TIME DEPENDENT; RIGHT ARROW: DIRECT PROBLEM; LEFT ARROW: INVERSE PROBLEM. STATISTICS PROBABILITY SEQUENTIAL STATISTICAL ANALYSIS OF RANDOM PROCESS IS THE KALMAN FILTER RANDOM PROCESS

15. THE WAY KALMAN FILTER WORKS THE KALMAN FILTER DOES MANY THINGS AND CAN BE INTERPRETED IN MANY WAYS AS SHOWN IN THE NEXT SET OF SLIDES STATE EQUATION (IMPROVED QUAL + QUAN) STATE NOISE (EFFECT IS SUPRESSED) MEASUREMENT EQUATION (IMPROVED QUAL + QUAN) MEASUREMENT NOISE

19. Mathematics of the Kalman filter- Bishop’s approach

20. Bishop and Welch - Kalman The state variable model for the Kalman filter. Insert equations 9 through 13 here : The first task during the measurement update is to compute the Kalman gain, The next step is to actually measure the process to obtain , and then to generate an a posteriori state estimate by incorporating the measurement as in (12). The final step is to obtain an a posteriori error covariance estimate via (1.13). The difference between the Kalman and Weiner filters is that the Kalman filter is recursive And at each time step the Kalman gain is computed.

21. Complete update of Kalman filter Each step require solving 5 equations. Nevertheless in modern day processors It is possible to solve the K filter equations either in SW as embedded code or in HW. There have been K filters designed for Systolic array based computation one of which we Will review in this session. Sorenson 1971- the quantity zk –Hxk is known in Kalman filtering as “residual”.

25. Setting up the plant >> A = [ 1.1269 -0.4940 0.1129; 1 0 0 ; 0 1 0 ]; >> A A = 1.1269 -0.4940 0.1129 1.0000 0 0 0 1.0000 0 >> B = [ -0.3832; 0.5919; 0.5191]; >> C = [ 1 0 0]; >> Bp = B'; >> Bp Bp = -0.3832 0.5919 0.5191 >> Plant = ss(A,[B B],C,0,-1,'inputname',{'u' 'w'}, 'outputname‘’,'y1‘,’y2’}); Q = 1; R = 1; [kalmf,L,P,K] = kalman(Plant,Q,R); >> K K = 0.3798 0.0817 -0.2570 K is really the Kalman gain

26. One we find the Kalman gain The inputs are process noise and output observations the outputs are state estimates x(n) and the estimate of the next observation. kalmf u yv ye Xhat(n) Compute the first output of the kalmf function kalmf = kalmf(1,:); Kalmf

27. Kalman filter outputs a = x1_e x2_e x3_e x1_e 0.7683 -0.494 0.1129 x2_e 0.6202 0 0 x3_e -0.081732 1 0 b = u y x1_e -0.3832 0.3586 x2_e 0.5919 0.3798 x3_e 0.5191 0.08173 c = x1_e x2_e x3_e y_e 0.6202 0 0 d = u y y_e 0 0.3798

28. Beginnings- Discrete time State variable model

29. Discrete Kalman filter Time update equations

30. Discrete Kalman filter Time update equations

31. When does Kalman gain converge quickly? The Kalman gain converges quickly when both the process noise covariance and error noise covariance are constant. (Q and R are Constant) One approach is to run the n filter off-line to estimate the degree of perturbation due to Q and R and arrive at an estimate of the Kalman gain . This is known as Grewal’s method(1993) When does the word recursive imply? Recursive implies does not imply all previous observations are stored and required to be processed every time a new data sample is received. This non requirement of storage Implies that the K filter requires computation than a Weiner filter

33. Whiteness

34. Continued 3. the assumption of Gaussian nature of noise - When a number of non-Gaussian RVs are added the result tends to Gaussian in the limit when the number of RVs tends to infinity. This is known as the strong form of the Central Limit theorem. While “whiteness” of measurement noise and process noise has to do with the PSD of noise , Gaussian nature implies the PDF of noise assumes a bell shaped curve for the range of amplitudes. Gaussian nature is independent of whiteness in fact it imposes and additional constraint on the noise. . Gaussian nature makes the mathematics more tractable as Gaussian PDF implies the first and second order conditional densities completely determine the observation and process noise.

39. The all important P matrix or error covariance matrix For each n , xhat(n-1|n-1) and P(n-1|n-1) are available. When a new observation y(n) occurs the new estimate for the state vector must be Computed and also the error covariance matrix P(n|n-1) is “propagated” To P(n|n). The evolution of the state vector follows the state variable model

40. Equations related a-priori and a-posteriori error

41. Step 2 : Incorporation of new measurement

42. Expression for updated estimate The next big conceptual jump the uncorrelated nature of the process noise And the measurement noise. E(v(n)w(n))= 0 ;E(v(n)x(n))= AE(v(n)E(x(n))+E(v(n)w(n))=0 This implies v(n) and x(n) are uncorrelated.

43. Error covariance matrix propagation Now we take the derivative of the error covariance matrix wrt. Kalman Gain

44. Final Expression for optimal gain

45. Closure: Plugging the optimal gain into error covariance For the final expression we plug the optimal Kalman gain value Into the error propagation covariance equation

46. Time and measurement updates for the continuous time Kalman filter The continuous time model of a linear stochastic system can be expressed as Follows.

47. Matlab Examples

51. Curves for 1-D Kalman Filter

52. Another simple K filter for position

53. Kalman demo 1 duration=5; dt=0.2; [pos,posmeas,poshat]= kalman_demo1(duration,dt); Kalman_demo1 function is available from the instructor

54. A 4x4 Kalman filter example This example includes a 4x4 estimator for a movement of a point in 2 –dimensional Plane starting from (10,10) . Consider a particle moving in the plane at constant velocity subject to random perturbations in its trajectory. The new position (x1, x2) is the old position plus the velocity (dx1, dx2) plus noise w. [ x1(t) ] = [1 0 1 0] [ x1(t-1) ] + [ wx1 ] [ x2(t) ] [0 1 0 1] [ x2(t-1) ] +[ wx2 ] [ dx1(t) ] [0 0 1 0] [ dx1(t-1) ] +[ wdx1 ] [ dx2(t) ] [0 0 0 1] [ dx2(t-1) ]+ [ wdx2 ] We assume we only observe the position of the particle. [ y1(t) ] = [1 0 0 0] [ x1(t) ] + [ vx1 ] [ y2(t) ] [0 1 0 0] [ x2(t) ]+ [ vx2 ] [ dx1(t) ] [ dx2(t) ] Plant = ss(A,[B B],C,0,-1,'inputname',{'u' 'w'}, 'outputname‘’,'y1‘,’y2’}); [x,y] = sample_lds(F, H, Q, R, initx, T); [xfilt, Vfilt, VVfilt, loglik] = kalman_filter(y, F, H, Q, R, initx, initV);

58. Building the F, H, G matrices T =.0.1sec; F= H = [ 1 0]; A = [ 1 0.1; 0 1]; B = [ 0.005; 0.1]; C = [ 1 0]; Xinit = [0;0];x = Xinit; xhat= Xinit; % Initital state estimate R = measnoise^2; % R matrix Sw = accelnoise^2*[ 1 0 ; 0 1]; % Q matrix P = Sw; % Defintion Position vector and Poistion estimated vectors pos = []; poshat = []; posmeas= []; vel = []; velhat = []; duration = 10;

60. Tracking error at two values of process noise This example is run for two different values of Process noise . The difference between The two estimates is telling. The second estimate of the position is much closer to the noisy measurement.

61. What are the key issues in implementation Verhaegen and Van Dooren proved in a legendary paper that the effectiveness of the K filter is dependent on the numerical accuracy with which the error covariance Matrix Or the P(n|n-1) matrix is represented. Loss of symmetry of the error Covariance matrix leads to seriosu issues with the implementation of the K. Filter. Numerical stability is exacerbated by due to the fact that the Riccatti Equation solution can get into trouble due to roundoff errors and the eignevalues Of the error covariance matrix can lead to a blowout of its condition number that is The ratio of the largest and smallest eignevalues. There many approaches to making the K filter more stable. No suprisingly one Approach – the Potter-Stern approach where P is not propagated but its Cholesky Transpose. P = CC’ Where C is propagated. Other approaches that have been tried is the QR decomposition and Bierman and Kailaths approaches. The most well known approach is called Square – root filters Which are reformulated K filters with better numerical stability

62. Equations to explain stability

64. Comparison of continuous time and discrete time Kalman filter equations

65. How do we relate the A.. F matrices? The best way is to use first order approximations to arrive at the difference from the Differential equations. The steps are not shown here but the final expressions are ,

69. 3 dimensional position tracking -an example

70. Full Form of A Matrix

71. How the matrix elements are derived

72. How the matrix elements are derived- Y axis This gives the second row of the A Matrix

73. Row 4 of the A matrix

74. Kalman filter for a second order system

75. Restructuring the DKF equations for HW implementation Reduces number of matrix multiplications by a quarter

76. THANKS

77. On Linear prediction for communications Mazumdar and Kadambi