################### VICON Settings
#equlibrium offset
#feedforwardMotor: [41000, 41000, 41000, 41000]

#quadratic motor regression equation (a0, a1, a2)
motorPoly: [5.484560e-4, 1.032633e-6, 2.130295e-11]

#feedback gain matrix original
#gainMatrixRoll: [0, -1.714330725, 0, 0, -1.337107465, 0, 5.115369735, 0, 0]
#gainMatrixPitch: [1.714330725, 0, 0, 1.337107465, 0, 0, 0, 5.115369735, 0]
gainMatrixYaw: [0, 0, 0, 0, 0, 0, 0, 0, 2.843099534]
#gainMatrixThrust: [0, 0, 0.22195826, 0, 0, 0.12362477, 0, 0, 0]

#kalman filter
filterGain: [1, 1, 1, 22.3384, 22.3384, 22.3384] #K_infinite of feedback
estimatorMatrix: [-22.3384, 0.9106] #only for velocity calculation

#setpoint in meters (x, y, z, yaw)
defaultSetpoint: [0.0, 0.0, 0, 0.0]

#take off and landing parameters (in meters and seconds)
takeOffDistance : 0.4
landingDistance : -0.5
durationTakeOff : 3
durationLanding : 3

# Liner Trayectory following parameters
#distanceThreshold : 0.5



################### UWB Settings
#equlibrium offset
feedforwardMotor: [43000, 43000, 43000, 43000]


#quadratic motor regression equation (a0, a1, a2)
#motorPoly: [5.484560e-4, 1.032633e-6, 2.130295e-11]

#feedback gain matrix changed
gainMatrixRoll: [0, -1.3, 0, 0, -0.9, 0, 4.715369735, 0, 0]
gainMatrixPitch: [1.3, 0, 0, 0.9, 0, 0, 0, 4.715369735, 0]
#gainMatrixYaw: [0, 0, 0, 0, 0, 0, 0, 0, 2.843099534]
gainMatrixThrust: [0, 0, 0.20195826, 0, 0, 0.08362477, 0, 0, 0]

#kalman filter
#filterGain: [1, 1, 1, 22.3384, 22.3384, 22.3384] #K_infinite of feedback
#estimatorMatrix: [-22.3384, 0.9106] #only for velocity calculation

#setpoint in meters (x, y, z, yaw)
#defaultSetpoint: [0.0, 0.0, 0.4, 0.0]

#take off and landing parameters (in meters and seconds)
#takeOffDistance : 0.4
#landingDistance : -0.5
#durationTakeOff : 3
#durationLanding : 3

# Liner Trayectory following parameters
distanceThreshold : 0.3