Commonly used algorithms for drones - Kalman filter (Part 2)

sigma

Commonly used algorithms for drones - Kalman filter (Part 2) [Copy link]

1.3 The Kalman Filter Algorithm In this section, we will describe the Kalman filter derived from Dr Kalman. The following description will involve some basic concepts, including probability, random variables, Gaussian distribution, and State-space Model, etc. However, the detailed proof of the Kalman filter cannot be described here one by one. First, we need to introduce a discrete control process system. The system can be described by a linear stochastic differential equation: X(k)=AX(k-1)+BU(k)+W(k) Add the measured value of the system: Z(k)=HX(k)+V(k) In the above two equations, X(k) is the system state at time k, and U(k) is the control quantity of the system at time k. A and B are system parameters. For multi-model systems, they are matrices. Z(k) is the measured value at time k, and H is the parameter of the measurement system. For multi-measurement systems, H is a matrix. W(k) and V(k) represent process and measurement noise respectively. They are assumed to be white Gaussian noise, and their covariances are Q and R respectively (here we assume that they do not change with the system state). For the above conditions (linear stochastic differential system, process and measurement are Gaussian white noise), Kalman filter is the optimal information processor. Let's use them and 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 process model of the system to predict the next state of the system. Assuming that the current system state is k, according to the system model, we can predict the state based on the previous state of the system: X(k|k-1)=AX(k-1|k-1)+BU(k) ……….. (1) In formula (1), X(k|k-1) is the result of the prediction using the previous state, X(k-1|k-1) is the optimal result of the previous state, and U(k) is the control amount of the current state. If there is no control amount, it can be 0. So far, our system results have been updated, but the covariance corresponding to X(k|k-1) has not been updated. We use P to represent covariance: P(k|k-1)=AP(k-1|k-1) A'+Q ……… (2) In formula (2), P(k|k-1) is the covariance corresponding to X(k|k-1), P(k-1|k-1) is the covariance corresponding to X(k-1|k-1), A' represents the transposed matrix of A, and Q is the covariance of the system process. Formulas 1 and 2 are the first two of the five formulas of the Kalman filter, which are the predictions of the system. Now that we have the prediction results of the current state, we can collect the measurement values of the current state. Combining the predicted value and the measured value, we can get the optimal estimate X(k|k) of the current state (k): X(k|k)= X(k|k-1)+Kg(k) (Z(k)-HX(k|k-1)) ……… (3) Where Kg is the Kalman gain (Kalman Gain): Kg(k)= P(k|k-1) H' / (HP(k|k-1) H' + R) ……… (4) So far, we have obtained the optimal estimate X(k|k) in state k. However, in order to keep the Kalman filter running until the system process ends, we also need to update the covariance of X(k|k) in state k: P(k|k)=（I-Kg(k) H）P(k|k-1) ……… (5) Where I is a matrix of 1, for a single model and single measurement, I=1. When the system enters the k+1 state, P(k|k) is P(k-1|k-1) in equation (2). In this way, the algorithm can operate autoregressively. The principle of the Kalman filter is basically described. Equations 1, 2, 3, 4 and 5 are its five basic formulas. Based on these five formulas, it is easy to implement a computer program. Next, I will use the program to give an example of actual operation. 1.4 Simple Example Here we combine the second and third sections to give a very simple example to illustrate the working process of the Kalman filter. The example given is to further describe the example in the second section, and it will also be accompanied by program simulation results. According to the description in the second section, the room is regarded as a system, and then this system is modeled. Of course, the model we see does not need to be very accurate. We know that the temperature of this room is the same as the temperature at the previous moment, so A=1. There is no control quantity, so U(k)=0. Therefore, it can be concluded that: X(k|k-1)=X(k-1|k-1) ………..(6) Equation (2) can be changed to: P(k|k-1)=P(k-1|k-1) +Q ……… (7) Because the measured value is from the thermometer and directly corresponds to the temperature, H=1. Formulas 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 the real temperature of the room is 25 degrees, I simulated 200 measurements with an average of 25 degrees, but added Gaussian white noise with a standard deviation of a few degrees (the blue line in the figure). In order to start the Kalman filter, we need to tell Kalman the initial values of two zero moments, which are X(0|0) and P(0|0). You don't need to care too much about their values, just give them one at random, because as Kalman works, X will gradually converge. But for P, generally don't take 0, because this may make Kalman completely believe that your given X(0|0) is the optimal system, so that the algorithm cannot converge. I chose X(0|0)=1 degree and P(0|0)=10. The actual temperature of the system is 25 degrees, which is represented by the black line in the figure. The red line in the figure is the optimal result of the Kalman filter output (the result is set to Q=1e-6 and R=1e-1 in the algorithm). Attached is the kalman filter program in matlab:

1. clear N=200; w(1)=0; w=randn(1,N) x(1)=0; 5 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; =a*s(t-1)+b(t)*(Y(t)-a*c*s(t-1)); p(t)=p1(t)-c*b(t)*p1(t); end t=1:N; plot(t,s,'r',t,Y,'g',t,x,'b');

复制代码

月下良缘

Thanks for sharing!