PID (/Kalman filter for better path following)

[LukeAnger1]

I've got basic pure pursuit implemented on my branch and I am thinking adding I'm PID to smooth out the navigation towards a point (kinda like an imaginary line)

[LukeAnger1]

Pure pursuit is working. We are going to have issues with with nav for lower speeds. We should implement PID to include IMU data to gain extra information.

Ex: Increasing goal lin/ang velocity on turns that are problematic (got stuck with .15 on one path but .4 works)

[LukeAnger1]

This would be better as a kalman filter, going to look into

[LukeAnger1]

@dvthomas01 PID has been added to path_library, passing mini sim test, need to run in lunar sim. Asking in slack if we can feed VIO data into kalman filter. Kalman filter is currently on a branch on my fork. Also want to be able to add in position estimates for each camera rather than averaging them. In my mind that makes more sense

[LukeAnger1]

Looking into these resources

https://www.researchgate.net/post/Does-anyone-have-a-6-DOF-IMU-Kalman-Filter
https://github.com/pykalman/pykalman

[LukeAnger1]

Found this for combining rotations

import numpy as np
from scipy.spatial.transform import Rotation

def quaternion_fusion_kalman(q1, q2, sigma1, sigma2):
# Convert standard deviations to weights (inverse of variance)
weight1 = 1.0 / (sigma12)
weight2 = 1.0 / (sigma22)

# Normalize weights
sum_weights = weight1 + weight2
weight1 /= sum_weights
weight2 /= sum_weights

# Convert quaternions to rotation objects
rot1 = Rotation.from_quat(q1)
rot2 = Rotation.from_quat(q2)

# Compute weighted interpolation (SLERP)
combined_rot = Rotation.slerp(rot1, rot2, weight2)

# Return as quaternion
return combined_rot.as_quat()

Found this for position

import numpy as np

def position_fusion(positions, std_devs):
"""
Fuse multiple position estimates based on their standard deviations.

Parameters:
positions: List of position vectors (each as [x, y, z])
std_devs: List of standard deviations for each position estimate

Returns:
Fused position vector
"""
positions = np.array(positions)
std_devs = np.array(std_devs)

# Calculate weights as inverse of variance (standard deviation squared)
weights = 1.0 / (std_devs**2)

# Normalize weights so they sum to 1
weights = weights / np.sum(weights)

# Apply weights to each position estimate
# Expand weights for element-wise multiplication with positions
weights_expanded = weights[:, np.newaxis]

# Weighted sum of positions
fused_position = np.sum(positions * weights_expanded, axis=0)

# Calculate the new combined standard deviation
# (This is the square root of the harmonic mean of the variances)
fused_variance = 1.0 / np.sum(1.0 / (std_devs**2))
fused_std_dev = np.sqrt(fused_variance)

return fused_position, fused_std_dev