Hi Venkata,
I understand that you are trying to calculate the upward position of a drone using body rates (p, q, r) provided by a bus creator. However, the body rates represent angular velocities - roll rate (p), pitch rate (q), and yaw rate (r) - not linear velocities or positions.
The body rates (p, q, r) represent the angular velocities of the drone about the body-fixed axes. The “r” value in this context is the yaw rate, or the rate of change of the drone's orientation about the vertical axis.
If you want to calculate the upward position of the drone, you would typically need the vertical velocity, not the yaw rate. The vertical velocity could be obtained by integrating the vertical acceleration (after subtracting gravity) if such data is available.
However, if you want to calculate the total change in yaw angle (which is different from the upward position), you can do this by integrating the “r” value over time. In MATLAB, this can be done using the ‘cumtrapz’ function, which computes the cumulative trapezoidal numerical integration. Here is a basic example:
% Assuming “r” is a vector of yaw rates and “t” is a vector of corresponding times[PG2] [AB3]
yaw_angle = cumtrapz(t, r);
This will give you a new vector, “yaw_angle”, which represents the total yaw angle at each point in time.
Please refer to the following MATLAB Documentation to know more about the following:
- ‘cumtrapz’ function: https://in.mathworks.com/help/matlab/ref/cumtrapz.html
- Integrator block: https://in.mathworks.com/help/simulink/slref/integrator.html
Hope the above suggestions help you.
Best regards,
Aman Banthia
