Kalman Filter/Majvik Filter

Question

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])

Answer 1

I programmed it by analogy with the site that you threw off. I don't really like the operation of this device with fluctuations. Because the angles jump strongly. I have an idea to make the alpha parameter dynamic, change it according to the gyroscope and accelerometer data. But so far I don't understand how to do it better.

import math
import serial
import time
import numpy as np

def calculate_angles_accel(accel):
    a_x, a_y, a_z = accel
    
    teta_x = math.atan2(a_y, a_z)
    teta_y = math.atan2(-a_x, (a_y ** 2 + a_z ** 2) ** .5)
    teta_z = 0
    
    return [teta_x, teta_y, teta_z]

def calculate_angles_magnetometer(magnet, angles_accel):
    m_x, m_y, m_z = magnet
    teta_x, teta_y, teta_z = angles_accel
    
    b_x = m_x * math.cos(teta_x) + m_y * math.sin(teta_x) * sin(teta_y) + m_z * math.sin(teta_x) * math.cos(teta_y)
    b_y = m_y * math.cos(teta_y) - m_z * math.sin(teta_y)
    b_z = -m_x * math.sin(teta_x) + m_y * math.cos(teta_x) * math.sin(teta_y) + m_z * math.cos(teta_x) * math.cos(teta_y)
    
    teta_z = math.atan2(-b_y, b_x)
    return [teta_x, teta_y, teta_z]

def calculate_angles_gyro(gyro, last_angles, delta_time):
    w_x, w_y, w_z = gyro
    teta_x, teta_y, teta_z = last_angles
    
    new_teta_x = teta_x + w_x * delta_time
    new_teta_y = teta_y + w_y * delta_time
    new_teta_z = teta_z + w_z * delta_time
    
    return [new_teta_x, new_teta_y, new_teta_z]

def complementary_filter(accel, gyro, last_angles, delta_time, alpha=0.2, magnet=[]):
    angles_accel = calculate_angles_accel(accel)
    
    if len(magnet) == 3: # if magnetometer turned on
        angles_accel = calculate_angles_accel(magnet, angles_accel)
    
    angles_gyro = calculate_angles_gyro(gyro, last_angles, delta_time)
    
    teta_x = alpha * angles_gyro[0] + (1 - alpha) * angles_accel[0]
    teta_y = alpha * angles_gyro[1] + (1 - alpha) * angles_accel[1]
    teta_z = alpha * angles_gyro[2] + (1 - alpha) * angles_accel[2]
    
    return [teta_x, teta_y, teta_z]

def convert_radians_to_degrees(angles):
    for i in range(len(angles)):
        angles[i] = round(math.degrees(angles[i]), 2)
    return angles

# Closing a port if it is open
try:
    ser.close()  
except:
    pass

ser = serial.Serial('COM4', 1000000) 

roll, pitch, yaw = 0, 0, 0
flag_0 = True

time_output = time.time()
start_time = time.time()

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
    
    if flag_0: # Setting the initial angles
        flag_0 = False
        roll, pitch, yaw = calculate_angles_accel(acceleration)
        continue
        # Add magnetometer if it necessary
    
    roll, pitch, yaw = complementary_filter(acceleration, gyroscope, [roll, pitch, yaw], delta_time) # Calculation of angles by a complementary filter
    
    roll_deg, pitch_deg, yaw_deg = convert_radians_to_degrees([roll, pitch, yaw]) # Converting angles from radians to degrees
    
    if time.time() - time_output > 0.01:
        time_output = time.time()
        padding = ' ' * 100
        print(f"{roll_deg}, {pitch_deg}, {yaw_deg}{padding}\r", end="")  

Answer 2

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.