Skip to content

Commit 6357c3a

Browse files
committed
restore some entries to the state dict
1 parent 06ae253 commit 6357c3a

File tree

1 file changed

+12
-3
lines changed

1 file changed

+12
-3
lines changed

roboticstoolbox/blocks/uav.py

Lines changed: 12 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -38,11 +38,18 @@ class MultiRotor(TransferBlock):
3838
vehicle state is a dict containing the following items:
3939
4040
- ``x`` pose in the world frame as :math:`[x, y, z, \theta_Y, \theta_P, \theta_R]`
41-
- ``vb`` translational velocity in the world frame (metres/sec)
42-
- ``w`` angular rates in the world frame as yaw-pitch-roll rates (radians/second)
41+
- ``trans`` position and velocity in the world frame as
42+
:math:`[x, y, z, \dot{x}, \dot{y}, \dot{z}]`
43+
- ``rot`` orientation and angular rate in the world frame as
44+
:math:`[\theta_Y, \theta_P, \theta_R, \dot{\theta_Y}, \dot{\theta_P}, \dot{\theta_R}]`
45+
- ``vb`` translational velocity in the body frame as
46+
:math:`[\dot{x}, \dot{y}, \dot{z}]`
47+
- ``w`` angular rates in the body frame as
48+
:math:`[\dot{\theta_Y}, \dot{\theta_P}, \dot{\theta_R}]`
4349
- ``a1s`` longitudinal flapping angles (radians)
4450
- ``b1s`` lateral flapping angles (radians)
45-
51+
- ``X`` full state vector as
52+
:math:`[x, y, z, \theta_Y, \theta_P, \theta_R, \dot{x}, \dot{y}, \dot{z}, \dot{\theta_Y}, \dot{\theta_P}, \dot{\theta_R}]`
4653
4754
The dynamic model is a dict with the following key/value pairs.
4855
@@ -263,6 +270,8 @@ def output(self, t, inports, x):
263270
out["x"] = x[0:6]
264271
out["trans"] = np.r_[x[:3], vd]
265272
out["rot"] = np.r_[x[3:6], rpyd]
273+
out["vb"] = np.linalg.inv(R) @ self._x[6:9] # translational velocity mapped to body frame
274+
out["w"] = iW @ self._x[9:12] # RPY rates mapped to body frame
266275

267276
out["a1s"] = self.a1s
268277
out["b1s"] = self.b1s

0 commit comments

Comments
 (0)