def compute_linearized_state_update_matrix(self, q, r):
""" Once we've updated the state estimates for phi and theta we need to linearize it again. [ISEFMAV 2.21] """
self.A[0, 0] = (q * cos(self.phihat) * safe_tangent(self.thetahat)) - (
r * sin(self.phihat) * safe_tangent(self.thetahat))
self.A[0, 1] = ((q * sin(self.phihat)) -
(r * cos(self.phihat))) / pow(cos(self.thetahat), 2)
self.A[1, 0] = (-q * sin(self.phihat)) + (r * cos(self.phihat))
def update_state_estimate_using_kinematic_update(self, p, q, r, dt):
""" When we get new gyro readings we can update the estimate of pitch and roll with the new values. [ISEFMAV 2.20] """
self.phihat = self.phi + (
(p + (q * sin(self.phi) * safe_tangent(self.theta)) +
(r * cos(self.phi) * safe_tangent(self.theta))) * dt)
self.thetahat = self.theta + (((q * cos(self.phi)) -
(r * sin(self.phi))) * dt)
logger.debug("kinematic update, phihat = %f, thetahat = %f" %
(self.phihat, self.thetahat))
def compute_linearized_state_update_matrix(self, q, r):
""" Once we've updated the state estimates for phi and theta we need to linearize it again. [ISEFMAV 2.21] """
self.A[0,0] = (q * cos(self.phihat) * safe_tangent(self.thetahat)) - (r * sin(self.phihat) * safe_tangent(self.thetahat))
self.A[0,1] = ((q * sin(self.phihat)) - (r * cos(self.phihat))) / pow(cos(self.thetahat), 2)
self.A[1,0] = (-q * sin(self.phihat)) + (r * cos(self.phihat))
def update_state_estimate_using_kinematic_update(self, p, q, r, dt):
""" When we get new gyro readings we can update the estimate of pitch and roll with the new values. [ISEFMAV 2.20] """
self.phihat = self.phi + ((p + (q * sin(self.phi) * safe_tangent(self.theta)) + (r * cos(self.phi) * safe_tangent(self.theta))) * dt)
self.thetahat = self.theta + (((q * cos(self.phi)) - (r * sin(self.phi))) * dt)
logger.debug("kinematic update, phihat = %f, thetahat = %f" % (self.phihat, self.thetahat))
