Balance car Kalman filter algorithm use experience
This time, we're focusing on the balance car and how it uses a Kalman filter. Let's dive into the experience and understanding of this powerful algorithm.
We use angular velocity sensors and accelerometers to measure both the angle and angular velocity of the system. However, because the car is in motion, the angle derived from acceleration isn't entirely accurate. Additionally, the angular velocity sensor readings are also affected by noise, leading us to question their reliability. To address this, we need to filter the data using the relationship between these two sensors. This is where the Kalman filter comes into play—it helps us combine these measurements to get a more accurate result.
To understand the Kalman filter, let’s start with a simple example. Imagine you want to estimate the temperature of a room. Based on your experience, you believe the temperature remains constant over time. However, you’re not 100% certain, so you consider this uncertainty as white Gaussian noise. You also place a thermometer in the room, but it's not perfectly accurate either. Its readings also contain noise.
At any given time step, you have two temperature estimates: one from your prior knowledge (the prediction) and another from the thermometer (the measurement). The Kalman filter helps you combine these two values, taking into account their respective uncertainties, to arrive at a more accurate estimate of the actual temperature.
Let’s say at time k-1, your predicted temperature was 23°C, with an uncertainty (standard deviation) of 5°C. At time k, the thermometer reads 25°C, with an uncertainty of 4°C. Using the Kalman gain formula:
$$ K_g = \frac{P_{\text{pred}}}{P_{\text{pred}} + P_{\text{meas}}} $$
where $ P_{\text{pred}} = 5^2 $ and $ P_{\text{meas}} = 4^2 $, we calculate $ K_g = 0.78 $. The updated estimate becomes:
$$ \text{Estimated Temperature} = 23 + 0.78 \times (25 - 23) = 24.56^\circ C $$
This shows that the filter gives more weight to the measurement when its uncertainty is lower.
After computing the optimal estimate, we update the covariance matrix for the next iteration. The new uncertainty is calculated as:
$$ \sqrt{(1 - K_g) \times P_{\text{pred}}} = 2.35 $$
This process continues recursively, allowing the Kalman filter to adapt and refine its estimates over time.
Now, let's look at the code implementation. This code is adapted from online sources and is used for our AVR microcontroller.
```c
#include "Kalman.h"
float Q_angle = 0.001, Q_gyro = 0.003, R_angle = 0.5, dt = 0.005;
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) {
angle += (gyro_m - q_bias) * dt; // Predict the next state
Pdot[0] = Q_angle - P[0][1] - P[1][0];
Pdot[1] = -P[1][1];
Pdot[2] = -P[1][1];
Pdot[3] = Q_gyro;
P[0][0] += Pdot[0] * dt;
P[0][1] += Pdot[1] * dt;
P[1][0] += Pdot[2] * dt;
P[1][1] += Pdot[3] * dt;
angle_err = angle_m - angle; // Measurement residual
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;
P[0][0] -= K_0 * PCt_0;
P[0][1] -= K_0 * PCt_1;
P[1][0] -= K_1 * PCt_0;
P[1][1] -= K_1 * PCt_1;
angle += K_0 * angle_err;
q_bias += K_1 * angle_err;
angle_dot = gyro_m - q_bias;
}
```
Each line in this code represents a key step in the Kalman filter algorithm. The first line predicts the next state based on the previous one. Then, the error covariance matrix is updated, followed by calculating the Kalman gain and updating the state estimate using the measurement.
The Kalman filter is particularly effective in real-time systems due to its recursive nature and low computational overhead. On an STM32 running at 72 MHz, this code executes in under 0.5 milliseconds, making it suitable for real-time applications like balancing robots.
In summary, the Kalman filter provides a robust way to combine noisy sensor data and improve the accuracy of system state estimation. Whether it's for temperature control, navigation, or balancing robots, this algorithm is a cornerstone of modern control systems.
Customizable Adapters,Custom Power Adapters,Customizable Usb Adapters,Thunderbolt Otg Adapter Cable
Dongguan Pinji Electronic Technology Limited , https://www.iquaxusb4cable.com