Skip to content

Commit

Permalink
Correct definition of Q_t in method update() of class EKF.
Browse files Browse the repository at this point in the history
  • Loading branch information
Mayitzin committed May 19, 2023
1 parent 99213a5 commit a214a7b
Showing 1 changed file with 1 addition and 1 deletion.
2 changes: 1 addition & 1 deletion ahrs/filters/ekf.py
Original file line number Diff line number Diff line change
Expand Up @@ -1391,7 +1391,7 @@ def update(self, q: np.ndarray, gyr: np.ndarray, acc: np.ndarray, mag: np.ndarra
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
Q_t = 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

0 comments on commit a214a7b

Please sign in to comment.