Source code for SEEL.SENSORS.ComplementaryFilter
[docs]class ComplementaryFilter:
def __init__(self,):
self.pitch=0
self.roll=0
self.dt=0.001
[docs] def addData(self,accData,gyrData):
self.pitch += (gyrData[0]) * self.dt # Angle around the X-axis
self.roll -= (gyrData[1]) * self.dt #Angle around the Y-axis
forceMagnitudeApprox = abs(accData[0]) + abs(accData[1]) + abs(accData[2]);
pitchAcc = np.arctan2(accData[1], accData[2]) * 180 / np.pi
self.pitch = self.pitch * 0.98 + pitchAcc * 0.02
rollAcc = np.arctan2(accData[0], accData[2]) * 180 / np.pi
self.roll = self.roll * 0.98 + rollAcc * 0.02
[docs] def getData(self):
return self.roll,self.pitch