Skip to content

utils.integration

Based on https://github.com/learnsyslab/crazyflow/blob/d28ec70d2478a230f1feb0fb3d644d1bbdbb8107/crazyflow/sim/integration.py.

Unlike crazyflow's version, this works with any array API backend and without rotor velocities.

integrate_EstimatorData(state, state_dot)

Integrates EstimatorData properly.

Source code in drone_estimators/utils/integration.py 
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
def integrate_EstimatorData(state: EstimatorData, state_dot: EstimatorData) -> EstimatorData:
    """Integrates EstimatorData properly."""
    next_pos, next_quat, next_vel, next_ang_vel, next_rotor_vel = _integrate(
        state.pos,
        state.quat,
        state.vel,
        state.ang_vel,
        state.rotor_vel,
        state_dot.pos,
        state.ang_vel,  # The orientation changes with the angular velocity
        state_dot.vel,
        state_dot.ang_vel,
        state_dot.rotor_vel,
        state.dt,
    )
    # TODO implement different integrator types later
    return state.replace(
        pos=next_pos, quat=next_quat, vel=next_vel, ang_vel=next_ang_vel, rotor_vel=next_rotor_vel
    )