def dynamics(self, state, control_input):
""" returns xdot """
[x,y,z,roll,pitch,yaw,dx,dy,dz,omegax,omegay,omegaz] = state
[w1,w2,w3,w4] = control_input
w1 = min(max(self.u_min[0],w1),self.u_max[0])
w2 = min(max(self.u_min[1],w2),self.u_max[1])
w3 = min(max(self.u_min[2],w3),self.u_max[2])
w4 = min(max(self.u_min[3],w4),self.u_max[3])
F1 = self.Kf*w1
F2 = self.Kf*w2
F3 = self.Kf*w3
F4 = self.Kf*w4
M1 = self.Km*w1
M2 = self.Km*w2
M3 = self.Km*w3
M4 = self.Km*w4
[ddx,ddy,ddz] = (1/self.m)*(array([0,0,-self.m*9.81]) + array(body2world([roll,pitch,yaw],[0,0,F1+F2+F3+F4])))
[alphax,alphay,alphaz] = dot(self.invI,(array([self.L*(F4-F2),self.L*(F3-F1),(M2+M4-M1-M3)])-cross([omegax,omegay,omegaz],dot(self.I,[omegax,omegay,omegaz]))))
[droll,dpitch,dyaw] = angularvel2rpydot([roll,pitch,yaw],[omegax,omegay,omegaz])
return [dx,dy,dz,droll,dpitch,dyaw,ddx,ddy,ddz,alphax,alphay,alphaz]
def dynamics(self, state, control_input):
""" returns xdot """
[x, y, z, roll, pitch, yaw, dx, dy, dz, omegax, omegay, omegaz] = state
[w1, w2, w3, w4] = control_input
w1 = min(max(self.u_min[0], w1), self.u_max[0])
w2 = min(max(self.u_min[1], w2), self.u_max[1])
w3 = min(max(self.u_min[2], w3), self.u_max[2])
w4 = min(max(self.u_min[3], w4), self.u_max[3])
F1 = self.Kf * w1
F2 = self.Kf * w2
F3 = self.Kf * w3
F4 = self.Kf * w4
M1 = self.Km * w1
M2 = self.Km * w2
M3 = self.Km * w3
M4 = self.Km * w4
[ddx, ddy,
ddz] = (1 / self.m) * (array([0, 0, -self.m * 9.81]) + array(
body2world([roll, pitch, yaw], [0, 0, F1 + F2 + F3 + F4])))
[alphax, alphay, alphaz
] = dot(self.invI, (array(
[self.L * (F4 - F2), self.L * (F3 - F1), (M2 + M4 - M1 - M3)]) -
cross([omegax, omegay, omegaz],
dot(self.I, [omegax, omegay, omegaz]))))
[droll, dpitch, dyaw] = angularvel2rpydot([roll, pitch, yaw],
[omegax, omegay, omegaz])
return [
dx, dy, dz, droll, dpitch, dyaw, ddx, ddy, ddz, alphax, alphay,
alphaz
]
import transforms if __name__=='__main__': rpy = [1.0, 2.0, 3.0] gyro = [1.5, 1.0, 1.5] rpydot = transforms.angularvel2rpydot(rpy, transforms.body2world(rpy, gyro)) rpydotexpected = [-2.109520760384187, -0.721904171343705, -3.969571070914463] for i in range(3): assert (rpydotexpected[i]-rpydot[i])<1E-6
def get_xhat(self):
if self._use_kalman:
dt = time.time() - self._last_kalman_update
self._kalman.predict(np.array(self._last_rpy + self._last_acc), dt)
if self._valid_vicon:
self._kalman.update(np.array(self._last_xyz))
self._last_kalman_update = time.time()
kalman_xhat = self._kalman.x.reshape(9).tolist()
xhat = [kalman_xhat[0],kalman_xhat[1],kalman_xhat[2],
self._last_rpy[0],self._last_rpy[1],self._last_rpy[2],
kalman_xhat[3],kalman_xhat[4],kalman_xhat[5],
self._last_gyro[0],self._last_gyro[1],self._last_gyro[2]]
# msg = dxyz_compare_t()
# msg.dxyzraw = self._last_dxyz
# msg.dxyzfiltered = kalman_xhat[3:6]
# self._xhat_lc.publish('dxyz_compare', msg.encode())
msg = kalman_args_t()
msg.input_rpy = self._last_rpy
msg.input_acc = self._last_acc
msg.input_dt = dt
msg.valid_vicon = self._valid_vicon
msg.meas_xyz = self._last_xyz
msg.smooth_xyz = self._last_xyz
msg.smooth_dxyz =self._last_dxyz
self._xhat_lc.publish('kalman_args', msg.encode())
else:
xhat = [self._last_xyz[0],self._last_xyz[1],self._last_xyz[2],
self._last_rpy[0],self._last_rpy[1],self._last_rpy[2],
self._last_dxyz[0],self._last_dxyz[1],self._last_dxyz[2],
self._last_gyro[0],self._last_gyro[1],self._last_gyro[2]]
if self._delay_comp:
control_inputs = self.get_last_inputs(self._delay)
for ci in control_inputs:
xhat = self._cf_model.simulate(xhat,ci[0],ci[1])
if self._use_rpydot:
try:
xhat[9:12] = angularvel2rpydot(self._last_rpy, body2world(self._last_rpy, self._last_gyro))
except ValueError:
xhat[9:12] = [0.0, 0.0, 0.0]
if self._publish_to_lcm:
msg = crazyflie_state_estimate_t()
msg.xhat = xhat
msg.t = self.get_time()
self._xhat_lc.publish("crazyflie_state_estimate", msg.encode())
# uncomment the following if you want to trigger hover at a predefined state
# if xhat[2] <= 0.15:
# print('This is how low')
# msg = crazyflie_controller_commands_t()
# msg.is_running = False
# self._xhat_lc.publish('crazyflie_controller_commands', msg.encode())
# msg = crazyflie_hover_commands_t()
# msg.hover = True
# self._xhat_lc.publish('crazyflie_hover_commands',msg.encode())
return xhat
import transforms
if __name__ == '__main__':
rpy = [1.0, 2.0, 3.0]
gyro = [1.5, 1.0, 1.5]
rpydot = transforms.angularvel2rpydot(rpy,
transforms.body2world(rpy, gyro))
rpydotexpected = [
-2.109520760384187, -0.721904171343705, -3.969571070914463
]
for i in range(3):
assert (rpydotexpected[i] - rpydot[i]) < 1E-6
