@@ -38,11 +38,18 @@ class MultiRotor(TransferBlock):
38
38
vehicle state is a dict containing the following items:
39
39
40
40
- ``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}]`
43
49
- ``a1s`` longitudinal flapping angles (radians)
44
50
- ``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}]`
46
53
47
54
The dynamic model is a dict with the following key/value pairs.
48
55
@@ -263,6 +270,8 @@ def output(self, t, inports, x):
263
270
out ["x" ] = x [0 :6 ]
264
271
out ["trans" ] = np .r_ [x [:3 ], vd ]
265
272
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
266
275
267
276
out ["a1s" ] = self .a1s
268
277
out ["b1s" ] = self .b1s
0 commit comments