Skip to content

Commit

Permalink
Merge pull request #57 from ManuelPalermo/master
Browse files Browse the repository at this point in the history 
add option to pass dt for each update step:
  • Loading branch information
@Mayitzin
Mayitzin committed Dec 27, 2021
2 parents 4953bfe + 67860bc commit 87f9210
Show file tree
Hide file tree
Showing 8 changed files with 76 additions and 35 deletions.
9 changes: 6 additions & 3 deletions ahrs/filters/angular.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -371,7 +371,7 @@ def _compute_all(self):
Q[t] = self.update(Q[t-1], self.gyr[t], method=self.method, order=self.order)
return Q

def update(self, q: np.ndarray, gyr: np.ndarray, method: str = 'closed', order: int = 1) -> np.ndarray:
def update(self, q: np.ndarray, gyr: np.ndarray, method: str = 'closed', order: int = 1, dt: float = None) -> np.ndarray:
"""Update the quaternion estimation
Estimate quaternion :math:`\\mathbf{q}_{t+1}` from given a-priori
Expand Down Expand Up @@ -408,6 +408,8 @@ def update(self, q: np.ndarray, gyr: np.ndarray, method: str = 'closed', order:
Estimation method to use. Options are: ``'series'`` or ``'closed'``.
order : int
Truncation order, if method ``'series'`` is used.
dt : float, default: None
Time step, in seconds, between consecutive Quaternions.
Returns
-------
Expand Down Expand Up @@ -436,6 +438,7 @@ def update(self, q: np.ndarray, gyr: np.ndarray, method: str = 'closed', order:
[-0.92504595, -0.23174096, 0.20086376, -0.22414251]])
"""
dt = self.Dt if dt is None else dt
if method.lower() not in ['series', 'closed']:
raise ValueError(f"Invalid method '{method}'. Try 'series' or 'closed'")
q = np.copy(q)
Expand All @@ -448,11 +451,11 @@ def update(self, q: np.ndarray, gyr: np.ndarray, method: str = 'closed', order:
[gyr[2], gyr[1], -gyr[0], 0.0]])
if method.lower() == 'closed':
w = np.linalg.norm(gyr)
A = np.cos(w*self.Dt/2.0)*np.eye(4) + np.sin(w*self.Dt/2.0)*Omega/w
A = np.cos(w*dt/2.0)*np.eye(4) + np.sin(w*dt/2.0)*Omega/w
else:
if order < 0:
raise ValueError(f"The order must be an int equal to or greater than 0. Got {order}")
S = 0.5 * self.Dt * Omega
S = 0.5 * dt * Omega
A = np.identity(4)
for i in range(1, order+1):
A += S**i / np.math.factorial(i)
Expand Down
14 changes: 10 additions & 4 deletions ahrs/filters/aqua.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -936,7 +936,7 @@ def estimate(self, acc: np.ndarray, mag: np.ndarray = None) -> np.ndarray:
return q/np.linalg.norm(q)
return q_acc

def updateIMU(self, q: np.ndarray, gyr: np.ndarray, acc: np.ndarray) -> np.ndarray:
def updateIMU(self, q: np.ndarray, gyr: np.ndarray, acc: np.ndarray, dt: float = None) -> np.ndarray:
"""
Quaternion Estimation with a IMU architecture.
Expand All @@ -958,18 +958,21 @@ def updateIMU(self, q: np.ndarray, gyr: np.ndarray, acc: np.ndarray) -> np.ndarr
Sample of tri-axial Gyroscope in rad/s.
acc : numpy.ndarray
Sample of tri-axial Accelerometer in m/s^2
dt : float, default: None
Time step, in seconds, between consecutive Quaternions.
Returns
-------
q : numpy.ndarray
Estimated quaternion.
"""
dt = self.Dt if dt is None else dt
if gyr is None or not np.linalg.norm(gyr) > 0:
return q
# PREDICTION
qDot = 0.5*self.Omega(gyr) @ q # Quaternion derivative (eq. 39)
qInt = q + qDot*self.Dt # Quaternion integration (eq. 42)
qInt = q + qDot*dt # Quaternion integration (eq. 42)
qInt /= np.linalg.norm(qInt)
# CORRECTION
a_norm = np.linalg.norm(acc)
Expand All @@ -984,7 +987,7 @@ def updateIMU(self, q: np.ndarray, gyr: np.ndarray, acc: np.ndarray) -> np.ndarr
q_prime = q_prod(qInt, q_acc) # (eq. 53)
return q_prime/np.linalg.norm(q_prime)

def updateMARG(self, q: np.ndarray, gyr: np.ndarray, acc: np.ndarray, mag: np.ndarray) -> np.ndarray:
def updateMARG(self, q: np.ndarray, gyr: np.ndarray, acc: np.ndarray, mag: np.ndarray, dt: float = None) -> np.ndarray:
"""
Quaternion Estimation with a MARG architecture.
Expand All @@ -1010,18 +1013,21 @@ def updateMARG(self, q: np.ndarray, gyr: np.ndarray, acc: np.ndarray, mag: np.nd
Sample of tri-axial Accelerometer in m/s^2
mag : numpy.ndarray
Sample of tri-axial Magnetometer in mT
dt : float, default: None
Time step, in seconds, between consecutive Quaternions.
Returns
-------
q : numpy.ndarray
Estimated quaternion.
"""
dt = self.Dt if dt is None else dt
if gyr is None or not np.linalg.norm(gyr)>0:
return q
# PREDICTION
qDot = 0.5*self.Omega(gyr) @ q # Quaternion derivative (eq. 39)
qInt = q + qDot*self.Dt # Quaternion integration (eq. 42)
qInt = q + qDot*dt # Quaternion integration (eq. 42)
qInt /= np.linalg.norm(qInt)
# CORRECTION
a_norm = np.linalg.norm(acc)
Expand Down
13 changes: 9 additions & 4 deletions ahrs/filters/complementary.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -183,7 +183,7 @@ def _compute_all(self) -> np.ndarray:
Q[t] = self.update(Q[t-1], self.gyr[t], self.acc[t], self.mag[t])
return Q

def attitude_propagation(self, q: np.ndarray, omega: np.ndarray) -> np.ndarray:
def attitude_propagation(self, q: np.ndarray, omega: np.ndarray, dt: float) -> np.ndarray:
"""
Attitude propagation of the orientation.
Expand All @@ -209,13 +209,15 @@ def attitude_propagation(self, q: np.ndarray, omega: np.ndarray) -> np.ndarray:
A-priori quaternion.
omega : numpy.ndarray
Tri-axial angular velocity, in rad/s.
dt : float
Time step, in seconds, between consecutive Quaternions.
Returns
-------
q_omega : numpy.ndarray
Estimated orientation, as quaternion.
"""
w = 0.5*self.Dt*omega
w = 0.5*dt*omega
A = np.array([
[1.0, -w[0], -w[1], -w[2]],
[w[0], 1.0, w[2], -w[1]],
Expand Down Expand Up @@ -304,7 +306,7 @@ def am_estimation(self, acc: np.ndarray, mag: np.ndarray = None) -> np.ndarray:
sz*cy*cx - cz*sy*sx])
return q_am / np.linalg.norm(q_am)

def update(self, q: np.ndarray, gyr: np.ndarray, acc: np.ndarray, mag: np.ndarray = None) -> np.ndarray:
def update(self, q: np.ndarray, gyr: np.ndarray, acc: np.ndarray, mag: np.ndarray = None, dt: float = None) -> np.ndarray:
"""
Attitude Estimation from given measurements and previous orientation.
Expand All @@ -328,16 +330,19 @@ def update(self, q: np.ndarray, gyr: np.ndarray, acc: np.ndarray, mag: np.ndarra
Sample of tri-axial Accelerometer in m/s^2.
mag : numpy.ndarray, default: None
Sample of tri-axial Magnetometer in uT.
dt : float, default: None
Time step, in seconds, between consecutive Quaternions.
Returns
-------
q : numpy.ndarray
Estimated quaternion.
"""
dt = self.Dt if dt is None else dt
if gyr is None or not np.linalg.norm(gyr)>0:
return q
q_omega = self.attitude_propagation(q, gyr)
q_omega = self.attitude_propagation(q, gyr, dt)
q_am = self.am_estimation(acc, mag)
# Complementary Estimation
q = (1.0 - self.gain)*q_omega + self.gain*q_am
Expand Down
25 changes: 16 additions & 9 deletions ahrs/filters/ekf.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -1105,7 +1105,7 @@ def Omega(self, x: np.ndarray) -> np.ndarray:
[x[1], -x[2], 0.0, x[0]],
[x[2], x[1], -x[0], 0.0]])

def f(self, q: np.ndarray, omega: np.ndarray) -> np.ndarray:
def f(self, q: np.ndarray, omega: np.ndarray, dt: float) -> np.ndarray:
"""Linearized function of Process Model (Prediction.)
.. math::
Expand All @@ -1123,16 +1123,18 @@ def f(self, q: np.ndarray, omega: np.ndarray) -> np.ndarray:
A-priori quaternion.
omega : numpy.ndarray
Angular velocity, in rad/s.
dt : float
Time step, in seconds, between consecutive Quaternions.
Returns
-------
q : numpy.ndarray
Linearized estimated quaternion in **Prediction** step.
"""
Omega_t = self.Omega(omega)
return (np.identity(4) + 0.5*self.Dt*Omega_t) @ q
return (np.identity(4) + 0.5*dt*Omega_t) @ q

def dfdq(self, omega: np.ndarray) -> np.ndarray:
def dfdq(self, omega: np.ndarray, dt: float) -> np.ndarray:
"""Jacobian of linearized predicted state.
.. math::
Expand All @@ -1148,13 +1150,15 @@ def dfdq(self, omega: np.ndarray) -> np.ndarray:
----------
omega : numpy.ndarray
Angular velocity in rad/s.
dt : float
Time step, in seconds, between consecutive Quaternions.
Returns
-------
F : numpy.ndarray
Jacobian of state.
"""
x = 0.5*self.Dt*omega
x = 0.5*dt*omega
return np.identity(4) + self.Omega(x)

def h(self, q: np.ndarray) -> np.ndarray:
Expand Down Expand Up @@ -1277,7 +1281,7 @@ def dhdq(self, q: np.ndarray, mode: str = 'normal') -> np.ndarray:
H = np.vstack((H, H_2))
return 2.0*H

def update(self, q: np.ndarray, gyr: np.ndarray, acc: np.ndarray, mag: np.ndarray = None) -> np.ndarray:
def update(self, q: np.ndarray, gyr: np.ndarray, acc: np.ndarray, mag: np.ndarray = None, dt: float = None) -> np.ndarray:
"""
Perform an update of the state.
Expand All @@ -1291,13 +1295,16 @@ def update(self, q: np.ndarray, gyr: np.ndarray, acc: np.ndarray, mag: np.ndarra
Sample of tri-axial Accelerometer in m/s^2.
mag : numpy.ndarray
Sample of tri-axial Magnetometer in uT.
dt : float, default: None
Time step, in seconds, between consecutive Quaternions.
Returns
-------
q : numpy.ndarray
Estimated a-posteriori orientation as quaternion.
"""
dt = self.Dt if dt is None else dt
if not np.isclose(np.linalg.norm(q), 1.0):
raise ValueError("A-priori quaternion must have a norm equal to 1.")
# Current Measurements
Expand All @@ -1315,10 +1322,10 @@ def update(self, q: np.ndarray, gyr: np.ndarray, acc: np.ndarray, mag: np.ndarra
self.z = np.r_[a, mag/m_norm]
self.R = np.diag(np.repeat(self.noises[1:] if mag is not None else self.noises[1], 3))
# ----- Prediction -----
q_t = self.f(q, g) # Predicted State
F = self.dfdq(g) # Linearized Fundamental Matrix
W = 0.5*self.Dt * np.r_[[-q[1:]], q[0]*np.identity(3) + skew(q[1:])] # Jacobian W = df/dω
Q_t = 0.5*self.Dt * self.g_noise * W@W.T # Process Noise Covariance
q_t = self.f(q, g, dt) # Predicted State
F = self.dfdq(g, dt) # Linearized Fundamental Matrix
W = 0.5*dt * np.r_[[-q[1:]], q[0]*np.identity(3) + skew(q[1:])] # Jacobian W = df/dω
Q_t = 0.5*dt * self.g_noise * W@W.T # Process Noise Covariance
P_t = F@self.P@F.T + Q_t # Predicted Covariance Matrix
# ----- Correction -----
y = self.h(q_t) # Expected Measurement function
Expand Down
7 changes: 5 additions & 2 deletions ahrs/filters/fourati.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -321,7 +321,7 @@ def _compute_all(self):
Q[t] = self.update(Q[t-1], self.gyr[t], self.acc[t], self.mag[t])
return Q

def update(self, q: np.ndarray, gyr: np.ndarray, acc: np.ndarray, mag: np.ndarray) -> np.ndarray:
def update(self, q: np.ndarray, gyr: np.ndarray, acc: np.ndarray, mag: np.ndarray, dt: float = None) -> np.ndarray:
"""
Quaternion Estimation with a MARG architecture.
Expand All @@ -335,13 +335,16 @@ def update(self, q: np.ndarray, gyr: np.ndarray, acc: np.ndarray, mag: np.ndarra
Sample of tri-axial Accelerometer in m/s^2
mag : numpy.ndarray
Sample of tri-axial Magnetometer in mT
dt : float, default: None
Time step, in seconds, between consecutive Quaternions.
Returns
-------
q : numpy.ndarray
Estimated quaternion.
"""
dt = self.Dt if dt is None else dt
if gyr is None or not np.linalg.norm(gyr)>0:
return q
qDot = 0.5 * q_prod(q, [0, *gyr]) # (eq. 5)
Expand All @@ -359,5 +362,5 @@ def update(self, q: np.ndarray, gyr: np.ndarray, acc: np.ndarray, mag: np.ndarra
K = self.gain*np.linalg.inv(X.T@X + lam*np.eye(3))@X.T # Filter gain (eq. 24)
Delta = [1, *K@dq] # Correction term (eq. 25)
qDot = q_prod(qDot, Delta) # Corrected quaternion rate (eq. 7)
q += qDot*self.Dt
q += qDot*dt
return q/np.linalg.norm(q)
14 changes: 10 additions & 4 deletions ahrs/filters/madgwick.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -557,7 +557,7 @@ def _compute_all(self) -> np.ndarray:
Q[t] = self.updateMARG(Q[t-1], self.gyr[t], self.acc[t], self.mag[t])
return Q

def updateIMU(self, q: np.ndarray, gyr: np.ndarray, acc: np.ndarray) -> np.ndarray:
def updateIMU(self, q: np.ndarray, gyr: np.ndarray, acc: np.ndarray, dt: float = None) -> np.ndarray:
"""
Quaternion Estimation with IMU architecture.
Expand All @@ -569,6 +569,8 @@ def updateIMU(self, q: np.ndarray, gyr: np.ndarray, acc: np.ndarray) -> np.ndarr
Sample of tri-axial Gyroscope in rad/s
acc : numpy.ndarray
Sample of tri-axial Accelerometer in m/s^2
dt : float, default: None
Time step, in seconds, between consecutive Quaternions.
Returns
-------
Expand Down Expand Up @@ -601,6 +603,7 @@ def updateIMU(self, q: np.ndarray, gyr: np.ndarray, acc: np.ndarray) -> np.ndarr
_assert_iterables(q, 'Quaternion')
_assert_iterables(gyr, 'Tri-axial gyroscope sample')
_assert_iterables(acc, 'Tri-axial accelerometer sample')
dt = self.Dt if dt is None else dt
if gyr is None or not np.linalg.norm(gyr) > 0:
return q
qDot = 0.5 * q_prod(q, [0, *gyr]) # (eq. 12)
Expand All @@ -619,11 +622,11 @@ def updateIMU(self, q: np.ndarray, gyr: np.ndarray, acc: np.ndarray) -> np.ndarr
gradient = J.T@f # (eq. 34)
gradient /= np.linalg.norm(gradient)
qDot -= self.gain*gradient # (eq. 33)
q += qDot*self.Dt # (eq. 13)
q += qDot*dt # (eq. 13)
q /= np.linalg.norm(q)
return q

def updateMARG(self, q: np.ndarray, gyr: np.ndarray, acc: np.ndarray, mag: np.ndarray) -> np.ndarray:
def updateMARG(self, q: np.ndarray, gyr: np.ndarray, acc: np.ndarray, mag: np.ndarray, dt: float = None) -> np.ndarray:
"""
Quaternion Estimation with a MARG architecture.
Expand All @@ -637,6 +640,8 @@ def updateMARG(self, q: np.ndarray, gyr: np.ndarray, acc: np.ndarray, mag: np.nd
Sample of tri-axial Accelerometer in m/s^2
mag : numpy.ndarray
Sample of tri-axial Magnetometer in nT
dt : float, default: None
Time step, in seconds, between consecutive Quaternions.
Returns
-------
Expand Down Expand Up @@ -671,6 +676,7 @@ def updateMARG(self, q: np.ndarray, gyr: np.ndarray, acc: np.ndarray, mag: np.nd
_assert_iterables(gyr, 'Tri-axial gyroscope sample')
_assert_iterables(acc, 'Tri-axial accelerometer sample')
_assert_iterables(mag, 'Tri-axial magnetometer sample')
dt = self.Dt if dt is None else dt
if gyr is None or not np.linalg.norm(gyr) > 0:
return q
if mag is None or not np.linalg.norm(mag) > 0:
Expand Down Expand Up @@ -701,6 +707,6 @@ def updateMARG(self, q: np.ndarray, gyr: np.ndarray, acc: np.ndarray, mag: np.nd
gradient = J.T@f # (eq. 34)
gradient /= np.linalg.norm(gradient)
qDot -= self.gain*gradient # (eq. 33)
q += qDot*self.Dt # (eq. 13)
q += qDot*dt # (eq. 13)
q /= np.linalg.norm(q)
return q

1 comment on commit 87f9210

@k323r
Copy link

@k323r k323r commented on 87f9210 Jan 5, 2022

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Thanks for this update, I was planning to implement it myself and do a pull request. Saved a ton of work :)

Please sign in to comment.