Kalman Estimator Diverge with flowdeck + Problem with rotation

[fbenti]

Hi, I encouter the following problems.

1. really often the kalman estimator starts diverging before taking off, when the drone is still on the ground. I would like to know if is possible to reset the kalman estimator at runtime, so that I could run in sequence: reset - takeoff- reset to see if that helps.

2. when using the goTo command, each crazyflie is capable of turning. Instead when I use the cmdPosition command, it seems like that the yaw parameter has no effect at all since nothing happens.

Here below you can see how I'm using the two command.

goTo

for cf in allcfs.crazyflies:
     cf.goTo(goal=np.array([0,0,HOVER_HEIGHT]),yaw=np.pi/2,duration=4)
timeHelper.sleep(5)

cmdPosition

timer = time.time()
while timer + 5 > time.time():
     for cf in allcfs.crazyflies:
           cf.cmdPosition(np.array([0,0,HOVER_HEIGHT]),yaw=np.pi/2)
     timeHelper.sleepForRate(50)

[whoenig]

For new projects, please consider Crazyswarm2 instead.

1. Yes, you can achieve that by setting the parameter "kalman.resetEstimation" to 1. For mocap,this is done automatically, see https://github.com/USC-ACTLab/crazyswarm/blob/master/ros_ws/src/crazyswarm/src/crazyswarm_server.cpp#L643-L644.
2. When looking at the firmware, the controller might expect deg, not rad as documented. Perhaps try with yaw=90 instead.

[fbenti]

Thanks. But what's the exact command to use to reset the estimator?

This is what I'm trying to do:

for cf in allcfs.crazyflies:
        dictKalman = cf.getParam("kalman")
        dictKalman["resetEstimation"] = 0
        cf.setParams(dictKalman)

but I get this error:

  File "SphereCoverage_waypoints.py", line 121, in <module>
    main()
  File "SphereCoverage_waypoints.py", line 73, in main
    cf.setParams(dictKalman)
  File "/home/aut/crazyswarm/ros_ws/src/crazyswarm/scripts/pycrazyswarm/crazyflie.py", line 384, in setParams
    self.updateParamsService(params.keys())
  File "/opt/ros/noetic/lib/python3/dist-packages/rospy/impl/tcpros_service.py", line 442, in __call__
    return self.call(*args, **kwds)
  File "/opt/ros/noetic/lib/python3/dist-packages/rospy/impl/tcpros_service.py", line 523, in call
    responses = transport.receive_once()
  File "/opt/ros/noetic/lib/python3/dist-packages/rospy/impl/tcpros_base.py", line 742, in receive_once
    p.read_messages(b, msg_queue, sock) 
  File "/opt/ros/noetic/lib/python3/dist-packages/rospy/impl/tcpros_service.py", line 360, in read_messages
    self._read_ok_byte(b, sock)
  File "/opt/ros/noetic/lib/python3/dist-packages/rospy/impl/tcpros_service.py", line 343, in _read_ok_byte
    raise ServiceException("service [%s] responded with an error: %s"%(self.resolved_name, str))
rospy.service.ServiceException: service [/cf1/update_params] responded with an error: b'basic_string::_M_create'

[whoenig]

See, for example, https://github.com/USC-ACTLab/crazyswarm/blob/master/ros_ws/src/crazyswarm/scripts/led_colors.py#L15-L16.

[knmcguire]

also a comment here, make sure that you have the PID controller instead of mellinger on for flying with the flowdeck. I've noticed that it flies off with that controller even if the estimate is good.