Kanlman filter learning (Principles and simple routines)

Kalman Filter
Kalman full name Rudolf Emil Kalman, a Hungarian mathematician, was born in Budapest, Hungary on 1930. In, he obtained a bachelor's degree and a Master's degree in electrical engineering from MIT. In 1957, he obtained a doctorate from Columbia University. The kalman filter that we need to learn in modern control theory, it is because of his doctoral thesis and the paper a new approach to linear filtering and prediction problems published in 1960 (a new method for linear filtering and prediction ). 1. Kalman Filter Algorithm In this section, we will describe the Kalman Filter derived from Dr Kalman. The following description involves some basic conceptual knowledge, including probability, random variable, Gaussian or normal distribution, and state-space model. However, the detailed proof of the Kalman filter cannot be described here. First, we need to introduce a discrete control process system. The system can be described by a linear random differential equation (Linear Stochastic difference equation: X (K) = a x (k-1) + B u (k) + W (k) Plus the system measurement value: Z (K) = H x (k) + V (k) In the previous two formulas, x (k) is the system state at K moment, and U (k) is the control value for the system at K moment. A and B are system parameters. For multi-model systems, they are matrices. Z (k) is the measured value at K time, and H is the parameter of the measurement system. H is the matrix of multiple measurement systems. W (K) and V (k) represent the process and measurement noise respectively. They are assumed to be white Gaussian noise and Their covariance is Q and R (Here we assume they do not change with the system status ). Kalman filter is the optimal information processor to meet the above conditions (process and measurement of linear random differential systems are Gaussian white noise. Next we will use them together with their covariances to estimate the optimal output of the system (similar to the temperature example in the previous section ). First, we need to use the system process model to predict the next state system. Assume that the current system status is K. Based on the system model, you can predict the status based on the previous status of the system: X (k | k-1) = a x (K-1 | k-1) + B u (k )........... (1) In formula (1), x (k | k-1) is the result of using the previous state prediction, x (K-1 | k-1) is the result of the optimum of the previous State, u (k) the current State control. If there is no control, it can be 0. Our system results have been updated so far, but the covariance for X (k | k-1) has not been updated yet. We use P to represent covariance: P (k | k-1) = a p (K-1 | k-1) a' + q ......... (2) In formula (2), P (k | k-1) is the covariance of X (k | k-1), P (K-1 | k-1) is the covariance of X (K-1 | k-1, a' indicates the transpose matrix of A, and Q is the covariance of the system process. Formula 1 and 2 are the first two of the five formulas of Kalman filter, that is, the prediction of the system. Now we have the prediction result of the current state, and then we collect the measurement value of the current state. Combined with the predicted values and measured values, we can obtain the optimal estimation value of the current State (k) x (k | K ): X (k | K) = x (k | k-1) + KG (k) (Z (k)-H x (k | k-1 ))......... (3) Where kg is Kalman gain (Kalman gain ): Kg (K) = P (k | k-1) H '/(h p (k | k-1) H' + r )......... (4) So far, we have obtained the optimal estimate value x (k | K) in K State ). But to keep the other Kalman filter running until the system process ends, we need to update the covariance of X (k | K) in K State: P (k | K) = (I-kg (k) H) P (k | k-1 )......... (5) Where I is a matrix of 1, for a single measurement of a single model, I = 1. When the system enters the k + 1 State, P (k | K) is the formula (2) P (K-1 | k-1 ). In this way, the algorithm can be used to perform self-regression operations. The principle of Kalman filter is basically described. Formula 1, 2, 3, 4, and 5 are his five basic formulas. According to these five formulas, it is easy to implement computer programs. Next, I will use a program to give an example of actual operation. 2. Simple Example Here we use section 3 of section 2 to give a very simple example to illustrate the operation process of Kalman filter. The example above is a further example of section 2, which is also matched with a program simulation result. According to the description in section 2, the room is regarded as a system, and then the system is modeled. Of course, the model we see does not need to be very accurate. The temperature of the room we know is the same as that of the previous time, so a = 1. There is no control, so u (K) = 0. Therefore, it is concluded that: X (k | k-1) = x (K-1 | k-1 )........... (6) The sub-Statement (2) can be changed: P (k | k-1) = P (K-1 | k-1) + q ......... (7) Because the measured value is a thermometer and corresponds directly to the temperature, H = 1. Format 3, 4, and 5 can be changed to the following: X (k | K) = x (k | k-1) + KG (k) (Z (k)-X (k | k-1 ))......... (8) Kg (K) = P (k | k-1)/(P (k | k-1) + r )......... (9) P (k | K) = (1-kg (k) P (k | k-1 )......... (10) Now we simulate a set of measurements as input. Assuming that the actual temperature in the room is 25 degrees, I have simulated 200 measurements with an average value of 25 degrees, however, the white Gaussian noise with a standard deviation of several degrees is added (Blue Line in the figure ). In order for the Kalman filter to start working, we need to tell the initial values of the Kalman two zero-hour moments: x (0 | 0) and P (0 | 0 ). They don't need to care too much about their values. Just give them one, because X will gradually converge with Kalman's work. However, for P, do not take 0, because it may make Kalman fully believe that the x (0 | 0) You have given is the optimal system, so that the algorithm cannot converge. I selected x (0 | 0) = 1 degree, P (0 | 0) = 10. The actual temperature of the system is 25 degrees, which is expressed by a black line in the figure. The red line in the figure is the optimal result output by the Kalman Filter (q = 1e-6, r = 1e-1 is set in the algorithm ).   Clear N = 200; W (1) = 0; W = randn (1, N) X (1) = 0; A = 1; For k = 2: N; X (K) = A * X (k-1) + W (k-1 ); End V = randn (1, N ); Q1 = STD (v ); Rvv = Q1. ^ 2; Q2 = STD (X ); Rxx = q2. ^ 2; Q3 = STD (w ); Rww = Q3. ^ 2; C = 0.2; Y = C * x + V; P (1) = 0; S (1) = 0; For t = 2: N; P1 (t) = A. ^ 2 * P (t-1) + rww; % P (k | k-1) = A * P (K-1 | k-1) a' + q B (t) = C * P1 (T)/(c. ^ 2 * P1 (t) + rvv); % Kalman gain matrix kg = P (k | k-1) H '/(h * P (k | k-1) + r) S (t) = A * S (t-1) + B (t) * (Y (t)-A * C * s (t-1 )); % optimum State X (k | K) = A * X (k | k-1) + KG (k) * (Z (k)-H * X (k | k-1 )) P (t) = p1 (t)-C * B (t) * P1 (t); % updated covariance P (k | K) = (I-kg (k) * h) * P (k | k-1) End T = 1: N; Plot (t, s, 'R', T, Y, 'G', t, x, 'B '); The kalman filter program made by MATLAB is used to pass the test   Kalman filter source program # Include "stdlib. H" Int klman (n, m, K, F, Q, R, H, Y, X, P, g) Int n, m, K; Double f [], Q [], R [], H [], Y [], X [], p [], G []; {Int I, j, KK, II, L, JJ, JS; Double * E, * a, * B; Extern int brinv (); E = malloc (M * m * sizeof (double )); L = m; If (L <n) L = N; A = malloc (L * l * sizeof (double )); B = malloc (L * l * sizeof (double )); For (I = 0; I <= n-1; I ++) For (j = 0; j <= n-1; j ++) {II = I * L + J; A [II] = 0.0; For (KK = 0; KK <= n-1; KK ++) A [II] = A [II] + P [I * n + KK] * f [J * n + KK]; } For (I = 0; I <= n-1; I ++) For (j = 0; j <= n-1; j ++) {II = I * n + J; P [II] = Q [II]; For (KK = 0; KK <= n-1; KK ++) P [II] = P [II] + F [I * n + KK] * A [Kk * L + J]; } For (II = 2; II <= K; II ++) {For (I = 0; I <= n-1; I ++) For (j = 0; j <= m-1; j ++) {JJ = I * L + J; A [JJ] = 0.0; For (KK = 0; KK <= n-1; KK ++) A [JJ] = A [JJ] + P [I * n + KK] * H [J * n + KK]; } For (I = 0; I <= m-1; I ++) For (j = 0; j <= m-1; j ++) {JJ = I * m + J; E [JJ] = R [JJ]; For (KK = 0; KK <= n-1; KK ++) E [JJ] = E [JJ] + H [I * n + KK] * A [Kk * L + J]; } JS = brinv (E, M ); If (JS = 0) {Free (E); free (a); free (B); Return (JS );} For (I = 0; I <= n-1; I ++) For (j = 0; j <= m-1; j ++) {JJ = I * m + J; G [JJ] = 0.0; For (KK = 0; KK <= m-1; KK ++) G [JJ] = G [JJ] + A [I * L + KK] * E [J * m + KK]; } For (I = 0; I <= n-1; I ++) {JJ = (II-1) * n + I; X [JJ] = 0.0; For (j = 0; j <= n-1; j ++) X [JJ] = x [JJ] + F [I * n + J] * X [(II-2) * n + J]; } For (I = 0; I <= m-1; I ++) {JJ = I * l; B [JJ] = Y [(II-1) * m + I]; For (j = 0; j <= n-1; j ++) B [JJ] = B [JJ]-H [I * n + J] * X [(II-1) * n + J]; } For (I = 0; I <= n-1; I ++) {JJ = (II-1) * n + I; For (j = 0; j <= m-1; j ++) X [JJ] = x [JJ] + G [I * m + J] * B [J * l]; } If (II <K) {For (I = 0; I <= n-1; I ++) For (j = 0; j <= n-1; j ++) {JJ = I * L + J; A [JJ] = 0.0; For (KK = 0; KK <= m-1; KK ++) A [JJ] = A [JJ]-G [I * m + KK] * H [Kk * n + J]; If (I = J) A [JJ] = 1.0 + A [JJ]; } For (I = 0; I <= n-1; I ++) For (j = 0; j <= n-1; j ++) {JJ = I * L + J; B [JJ] = 0.0; For (KK = 0; KK <= n-1; KK ++) B [JJ] = B [JJ] + A [I * L + KK] * P [Kk * n + J]; } For (I = 0; I <= n-1; I ++) For (j = 0; j <= n-1; j ++) {JJ = I * L + J; A [JJ] = 0.0; For (KK = 0; KK <= n-1; KK ++) A [JJ] = A [JJ] + B [I * L + KK] * f [J * n + KK]; } For (I = 0; I <= n-1; I ++) For (j = 0; j <= n-1; j ++) {JJ = I * n + J; P [JJ] = Q [JJ]; For (KK = 0; KK <= n-1; KK ++) P [JJ] = P [JJ] + F [I * n + KK] * A [J * L + KK]; } } } Free (E); free (a); free (B ); Return (JS ); } From: http://jpkc.nwpu.edu.cn/jp2005/40/ebook/kcsj/chp1/Kalman.htm Useful link: http://www.cs.unc.edu /~ Welch/Kalman/