This time, the balance car, using Kalman filter, let's talk about using the experience.
We use angular velocity sensors and accelerometers to measure the angular and angular velocities, but since the car is moving, the angle we use for acceleration is not completely correct. Due to noise interference, we also have doubts about the angular velocity sensor measurements. So we have to filter, through the relationship between the two sensors to get the results we want. We use a Kalman filter to connect these two measurements.
First of all, openly understand the understanding of Kalman, quote the online (Baidu Encyclopedia) classic interpretation:
Before introducing his five formulas, let us explore step by step according to the following examples.
Suppose we are going to study the temperature of a room. According to your experience, the temperature of this room is constant, that is, the temperature of the next minute is equal to the temperature of the current minute (assuming we use one minute to do the time unit). Suppose you are not 100% confident in your experience, there may be a few degrees of deviation. We consider these deviations as white Gaussian Noise, which means that these deviations are independent of the time before and after and are consistent with Gaussian Distribution. In addition, we put a thermometer in the room, but this thermometer is not accurate, the measured value will deviate from the actual value. We also consider these deviations as Gaussian white noise.
Ok, now for a minute we have two temperature values ​​for the room: your predicted value based on experience (predicted by the system) and the value of the thermometer (measured value). Below we will use these two values ​​combined with their respective noise to estimate the actual temperature of the room.
Suppose we want to estimate the actual temperature value at time k. First, you should predict the temperature at time k based on the temperature at time k-1. Because you believe that the temperature is constant, you will get the temperature prediction at time k that is the same as at k-1, assuming 23 degrees, and the Gaussian noise of the value is 5 degrees (5 is the result: If the deviation of the optimal temperature value estimated at time k-1 is 3, the uncertainty of your own prediction is 4 degrees, and they are squared and then squared, which is 5). Then, you get the temperature value at time k from the thermometer, assuming 25 degrees, and the deviation of the value is 4 degrees.
Since we used to estimate the actual temperature at time k, there are two temperature values, 23 degrees and 25 degrees, respectively. What is the actual temperature? Believe that I still believe in the thermometer? If you believe more, we can use their covariance to judge. Since Kg^2=5^2/(5^2+4^2), so Kg=0.78, we can estimate the actual temperature value at time k: 23+0.78* (25-23)=24.56 degrees. It can be seen that because the covariance of the thermometer is relatively small (compared to the thermometer), the estimated optimal temperature value is biased towards the value of the thermometer.
Now that we have the optimal temperature value at time k, the next step is to enter the k+1 time and make a new optimal estimate. Until now, it seems that I have not seen anything about self-return. By the way, before entering the k+1 time, we have to calculate the deviation of the optimal value (24.56 degrees) at time k. The algorithm is as follows: ((1-Kg)*5^2)^0.5=2.35. Here 5 is the deviation of the 23 degree temperature value you predicted above at time k. The resulting 2.35 is the deviation of the optimal temperature value estimated at time k after entering k+1 (corresponding to 3 above).
In this way, the Kalman filter continually recursively covariance to estimate the optimal temperature value. He runs very fast, and it only retains the covariance of the last moment. The above Kg is the Kalman Gain. He can change his own value with different moments, is it amazing!
Then take a look at our code, the code comes from the network, using the code of our avr
#include "Kalman.h"
Float Q_angle=0.001, Q_gyro=0.003, R_angle=0.5, dt=0.005;
/ / Note: the value of dt is the sampling time of the kalman filter;
Float P[2][2] = {
{ 1, 0 },
{ 0, 1 }
};
Float Pdot[4] ={0,0,0,0};
Const char C_0 = 1;
Float q_bias, angle_err, PCt_0, PCt_1, E, K_0, K_1, t_0, t_1;
//------------------------------------------------ -------
Void Kalman_Filter(float angle_m,float gyro_m) //gyro_m:gyro_measure
{
Angle+=(gyro_m-q_bias) * dt;// a priori estimate
Pdot[0]=Q_angle - P[0][1] - P[1][0];// Pk-' Differential of the prior estimate error covariance
Pdot[1]=- P[1][1];
Pdot[2]=- P[1][1];
Pdot[3]=Q_gyro;
P[0][0] += Pdot[0] * dt;// Pk- Integral estimation error covariance differential integral = prior estimate error covariance
P[0][1] += Pdot[1] * dt;
P[1][0] += Pdot[2] * dt;
P[1][1] += Pdot[3] * dt;
Angle_err = angle_m - angle; / / zk - a priori estimate
PCt_0 = C_0 * P[0][0];
PCt_1 = C_0 * P[1][0];
E = R_angle + C_0 * PCt_0;
K_0 = PCt_0 / E;//Kk
K_1 = PCt_1 / E;
T_0 = PCt_0;
T_1 = C_0 * P[0][1];
P[0][0] -= K_0 * t_0;// a posteriori estimation error covariance
P[0][1] -= K_0 * t_1;
P[1][0] -= K_1 * t_0;
P[1][1] -= K_1 * t_1;
Angle += K_0 * angle_err;//posterior estimation
Q_bias += K_1 * angle_err;//posterior estimate
Angle_dot = gyro_m-q_bias; / / differential value of output value (a posteriori estimate) = angular velocity
}
We explain each sentence
Angle+=(gyro_m-q_bias) * dt
First we need to use the system's process model to predict the next state of the system. Assuming that the current system state is k, depending on the model of the system, it can be predicted to appear in the state based on the previous state of the system:
X(k|k-1)=AX(k-1|k-1)+BU(k) ........... (1)
Our matrix X is:
(angle
Gyro)
Our matrix A is:
( 1 1
0 1)
It should be noted that we get X(k|k-1)!! This is not the result we want.
After that
Pdot[0]=Q_angle - P[0][1] - P[1][0];// Pk-' Differential of the prior estimate error covariance
Pdot[1]=- P[1][1];
Pdot[2]=- P[1][1];
Pdot[3]=Q_gyro;
P[0][0] += Pdot[0] * dt;// Pk- Integral estimation error covariance differential integral = prior estimate error covariance
P[0][1] += Pdot[1] * dt;
P[1][0] += Pdot[2] * dt;
P[1][1] += Pdot[3] * dt;
These eight sentences are explained together
So far, our system results have been updated, but the covariance corresponding to X(k|k-1) has not been updated. We use P for covariance:
P(k|k-1)=AP(k-1|k-1) A'+Q ......... (2)
Pdot is the differential of P.
Our Q is
(Q_angle 0
0 Q_gyro)
The covariance after the integration is calculated, and the same is also noted as P(k|k-1). How to calculate the specific ~~~ Well, I admit that my line has not learned well~~~ For a long time. . . .
Angle_err = angle_m - angle;//This sentence does not seem to say ~~
Next, calculate the Kalman gain:
PCt_0 = C_0 * P[0][0];
PCt_1 = C_0 * P[1][0];
E = R_angle + C_0 * PCt_0;
K_0 = PCt_0 / E;
K_1 = PCt_1 / E;
Kg(k)= P(k|k-1) H' / (HP(k|k-1) H' + R)
H is the matrix of the measurement system, which is (1)
1)
T_0 = PCt_0;
T_1 = C_0 * P[0][1];
P[0][0] -= K_0 * t_0;// a posteriori estimation error covariance
P[0][1] -= K_0 * t_1;
P[1][0] -= K_1 * t_0;
P[1][1] -= K_1 * t_1;
Up to now, we have obtained the optimal estimate X(k|k) in the k state. But in order to continue the operation of the other Kalman filter until the end of the system process, we also need to update the covariance of X(k|k) in the k state:
P(k|k)=(I-Kg(k) H)P(k|k-1) ......... (5)
This is a good understanding of ~~I is the unit matrix does not say more birds ~~
Angle += K_0 * angle_err;//posterior estimation
Q_bias += K_1 * angle_err;//posterior estimate
Angle_dot = gyro_m-q_bias; / / differential value of output value (a posteriori estimate) = angular velocity
Now that we have the predictions for the current state, we then collect the measurements for the current state. Combining the predicted and measured values, 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)
Observe the K_1 calculation process, and then contact the nature of the covariance matrix to know why the angular velocity deviation is calculated with P[1][0]~~
As for the speed of running on STM32, the execution time of this code in 72M is within 0.5 milliseconds, the speed is not a problem~~
60V Battery Pack ,Battery Pack With Outlet,Back Up Battery Pack,Ev Battery Pack
Zhejiang Casnovo Materials Co., Ltd. , https://www.casnovonewenergy.com