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