1
$\begingroup$

I have logged gyroscope sensor data for roll(x), pitch(y) and yaw(z) axis and i want to plot their step response with Python to be able to tune the pid controller of my quad better.

1. I have found example python code for creating transfer function and taking step response. However, i don't know how to determine the transfer function and num, den parameters for a pid controller and how to apply the gyro data.

2. The pid controller also have feedforward as well. How the transfer function changes in this case.

In short, i want to simply convert the gyro data stored on an array to step response data and plot it. What is the correct and simple way of doing it.

Code example i have found is:

import numpy as np
import control
import matplotlib . pyplot as plt

# %% Creating model :
num = np . array ([2])
den = np . array ([5 , 1])
H = control . tf ( num , den )

# %% Defining signals :
t0 = 0
t1 = 20
dt = 0.01
nt = int ( t1 / dt ) + 1 # Number of points of sim time
t = np . linspace ( t0 , t1 , nt )
u = 2* np . ones ( nt )

# %% Simulation :
(t , y , x ) = control . forced_response (H , t , u , X0 =0)

# %% Plotting :
plt . close ( ’ all ’)
fig_width_cm = 24
fig_height_cm = 18
plt . figure (1 , figsize =( fig_width_cm /2.54 , fig_height_cm /2.54))
plt . subplot (2 , 1 , 1)
plt . plot (t , y , ’ blue ’)
# plt . xlabel ( ’ t [ s ] ’)
plt . grid ()
plt . legend ( labels =( ’y ’ ,))
plt . subplot (2 , 1 , 2)
plt . plot (t , u , ’ green ’)
plt . xlabel ( ’ t [ s ] ’)
plt . grid ()
plt . legend ( labels =( ’u ’ ,))

PID Tool Box Example: Step Response Gyro Data

$\endgroup$

1 Answer 1

0
$\begingroup$

Deriving Step Response from Input and Output Data

Deriving the transfer function from time domain input and output data for a closed loop system is much more challenging as the individual closed loop poles and zeros would need to be determined. However this is not necessary to get the desired step response. We can derive a impulse response directly and from that get the step response.

Given the OP has both the time domain input and output data of the system, the impulse response can be derived using the Wiener-Hopf equations which I detail here along with Python code. The code of interest is in the very beginning with the function

coeff = channel(tx,rx,ntaps)

coeff would be the resulting samples of the impulse response.
tx is the input signal.
rx is the output signal.
ntaps is the number of samples to use for the impulse response.

ntaps should be longer than the actual impulse response to to prevent time domain aliasing but if it gets too long, noise enhancement results. What I do is start with ntaps much longer than I think it will be (this is the expected response time, one could use 5 time constants or 5x the reciprocal of the bandwidth) and then I observe the resulting impulse response which if done right will be somewhat clustered near the center (otherwise adjust the time offset of the input related to the output in the equation to shift the result toward the center). From that I can view how wide in time the resulting impulse response is, and then I shorten ntaps to be closer to that.

This is an optimum least squares solution approach to determining the sampled impulse response for a linear system. In the linked post this was then used to determine group delay, but we can also get the step response by simply integrating the impulse response (accumulating the samples of the impulse response).

Some caution with this approach (and any approach to derive the frequency response): the input must be spectrally rich, meaning must include energy at all the frequencies where we wish to map out the frequency response. With the least squares approach, which is optimum, the resulting frequency response at any given frequency will have an accuracy that is proportional to the signal to noise ratio at that frequency. If we were to test a system with a single tone frequency for example, we can only really derive the frequency response for that frequency. Often we use for stimulus either a swept tone (like in a network analyzer) or pseudo-random noise signals which are spectrally white (power spectral density is constant across frequency). The suitability of a given input signal can be determined by observing its FFT to assess if the input signal is "white" enough for deriving a frequency response. If the signal has a spectral occupancy everywhere where a frequency response is desired that is sufficiently above the noise floor and other interference signals if they exist, then it will be suitable for deriving a transfer function with a fidelity proportional to that SNR.

Another approach is to derive the frequency response, and from the frequency response the impulse response is determined as the inverse Fourier Transform. This can be done by dividing the FFT of the output by the input and using the inverse FFT to get samples of the impulse response. This approach is inferior to using the least squares Wiener-Hopf equations as I have detailed with an example at this post. I copied the plots from this post below that provide a specific demonstration of the main points I am making here so that I can translate to the OP's context:

The first plot is the frequency spectrum determined from the input signal (as the FFT magnitude converted to dB), and notice that it is "spectrally rich" only in the frequency range of +/-0.5 MHz. Right away we see that we can only hope to derive a frequency response over this bandwidth from the time domain output resulting from the time domain input signal.

input spectrum

The main point I want to show with the next plot below is how well the determined frequency response matched the actual known frequency response in this case, only in the range of +/-0.5 MHz given that is the frequencies we have sufficiently "probed" the input to the system. "Channel Estimation" refers to the frequency response of the system that the input signal went through to get to the output measured time response. In that post where I copied these plots from, I was interested in viewing the frequency response, but this was determined from a time domain impulse response such as what the OP will need to get the step response.

Channel using Wiener Hopf

This final plot from that post shows the same result using an "FFT approach" which I would not recommend given its result compared to the Wiener-Hopf method.

Channel using FFT

Measuring the Step Response Directly

To measure the step response, provide a step change as an input stimulus and then measure the resulting response to that step. You cannot derive a response or transfer function without knowing both the input and output. The change should be large enough so that it’s response is significant compared to background noise and drift but not too large such as to cause the loop to no longer be working in its linear region (so as small as possible, just big enough to be visible). What I do is increase the step so that it is big enough to see, and repeat the test with successively larger steps and confirm a similar response shape occurs, until that is no longer the case.

Amy easy way to induce the input “step” in the loop is to sum a small step at the input to the loop filter, together with the error signal from the sensor. The step response can then be measured at the output of the loop filter.

$\endgroup$
1
  • $\begingroup$ ntaps is the number of samples in the resulting impulse response. What I do is start with it much longer than I think it will be (this is the expected response time, you could use 5 time constants or 5x the reciprocal of the bandwidth) and then I observe the resulting impulse response which if done right will be somewhat clustered near the center (otherwise adjust the time offset of the input related to the output in the equation to shift the result toward the center). From that I can view how wide in time the resulting impulse response is, and then I shorten ntaps to be closer to that. $\endgroup$ Jul 2, 2023 at 13:14

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.

Not the answer you're looking for? Browse other questions tagged or ask your own question.