I have an accelerometer sensor and a gyroscope sensor, both three-axis. I would have to somehow calculate the roll/pitch angles in dynamics according to the accelerometer and gyroscope readings, as well as the yaw angle according to the gyroscope readings (in the future I will add a magnetometer for correction).
To solve this problem, I decided to use the Kalman filter. Data from sensors is read by acceleration, gyroscope = get_data(), a list of 6 values is returned. Next, I consider the change of angles according to the data from the gyroscope delta_gyro_angle(gyroscope, delta_time). Then I calculate the angles using the accelerometer compute_roll_pitch(acceleration). After that, I try to combine them through the Kalman filter filter_angles(accel_angles, gyro_delta s, gyro_noise, accel_noise). But I have a feeling that my implementation of the kalman filter ignores the change of angles by gyroscope. I also have problems with rare, but large outliers of data from the accelerometer (for example, a 1.5-fold jump in a static state). In addition, I have problems when the pitch angle is close to 90 degrees. There are also problems with the angle +-180.
How can I solve these problems?
In short, I want to get angles based on the readings of the accelerometer and gyroscope while in motion, and most likely non-linear. I want to use this data in my radio-controlled toy boat.
import numpy as np
import time
import math
def normalize(vector):
norm = np.linalg.norm(vector)
if norm == 0:
return vector
return vector / norm
def compute_roll_pitch(acceleration):
# Normalize acceleration
acceleration = normalize(acceleration)
# Calculate pitch angle (pitch) and roll angle (roll)
pitch = math.asin(-acceleration[0])
roll = math.atan2(acceleration[1], acceleration[2])
# Convert angles from radians to degrees
pitch_degrees = math.degrees(pitch)
roll_degrees = math.degrees(roll)
return roll_degrees, pitch_degrees
class KalmanFilter:
def __init__(self, initial_angles, gyro_noise, accel_noise):
# Initialization of matrices and filter parameters
self.angle_estimate = np.array(initial_angles) # Initial estimation of angles
self.angle_covariance = np.identity(3) # Initial covariance of angles
self.gyro_noise = gyro_noise
self.accel_noise = accel_noise
def update(self, gyro_deltas, accel_angles):
# Predicting the next filter state
predicted_angle = self.angle_estimate + gyro_deltas
predicted_covariance = self.angle_covariance + self.gyro_noise
# Calculation of the correction coefficient (Kalman coefficient)
kalman_gain = np.dot(predicted_covariance, np.linalg.inv(predicted_covariance + self.accel_noise))
# Updating the angle estimation based on the accelerometer readings
self.angle_estimate = predicted_angle + np.dot(kalman_gain, (accel_angles - predicted_angle))
self.angle_covariance = np.dot((np.identity(3) - kalman_gain), predicted_covariance)
return self.angle_estimate.tolist()
def filter_angles(accel_angles, gyro_deltas, gyro_noise, accel_noise):
# Creating a Kalman filter object
kalman_filter = KalmanFilter(accel_angles, gyro_noise, accel_noise)
# Angle filtering using gyroscope and accelerometer data
filtered_angles = [accel_angles]
for delta in gyro_deltas:
filtered_angle = kalman_filter.update(delta, accel_angles)
filtered_angles.append(filtered_angle)
return filtered_angles[-1]
roll, pitch, yaw = 0, 0, 0
time_output = time.time()
start_time = time.time()
total_roll = [] # Roll history
total_pitch = [] # Pitch history
total_data = [] # History of data from accelerometer and gyroscope
while True:
delta_time = time.time() - start_time # Time between iterations
start_time = time.time()
acceleration, gyroscope = get_data() # Getting data from accelerometer and gyroscope
delta_roll, delta_pitch, delta_yaw = delta_gyro_angle(gyroscope, delta_time) # Calculation of angle changes by gyroscope
roll_accel, pitch_accel = compute_roll_pitch(acceleration) # Calculating angles by accelerometer
yaw_accel = yaw # add a magnetometer in future.
accel_angles = [roll_accel, pitch_accel, yaw_accel] # Accelerometer-based angles (roll, pitch, yaw)
gyro_deltas = [delta_roll, delta_pitch, delta_yaw] # Angular gyroscope changes (roll delta, pitch delta, yaw delta)
gyro_noise = np.identity(3) * 0.00001 # Gyroscope noise matrix
accel_noise = np.identity(3) * 0.001 # Accelerometer noise matrix
roll, pitch, yew = filter_angles(accel_angles, gyro_deltas, gyro_noise, accel_noise)
if time.time() - time_output > 0.01:
time_output = time.time()
padding = ' ' * 100
print(f"{yaw}, {pitch}, {roll}{padding}\r", end="")
total_roll.append(roll)
total_pitch.append(pitch)
total_data.append([acceleration, gyroscope])
I do not think your approach of updating with gyro measurement, and doing a correction to that using accelerometer measurement is the right approach here. A typical Kalman filter setup uses gyro for attitude prediction, and accelerometer for position prediction. Then uses external sensors such as GNSS as a correction.
If you are going to use this system on a boat, which is a high dynamic system, your accelerometers are going to detect impacts and high-acceleration measurements as body-accelerations, and going to give you wrong angle measurements. This is also will be true for gyro measurements.
I think a better approach here would be to use something like a complimentary filter. See an example implementation here.