This blog is Part II of a two part series showing how to implelement system identification on an embedded MCU. Part I is here. The specific example shown here identifies the velocity loop of a BLDC motor control that uses Field Oriented Control (FOC). The MCU used is a STM32G431 Cortex.
Block Diagram
A block diagram of the general control loop is shown below:
Figure 1 - Control Loop Diagram
I will expand on the details of this block later but for now the things to know are y(k) could be the output velocity. The input ref(k) could be the reference velocity command. The objective is to excite the system at ref(k) and record the response y(k).
Comparing this to what was discussed in Part 1 the excitation u(k) is added to ref(k) to drive the closed loop. The output of the loop is taken at y(k). With this information the transfer function from u(k) --> y(k) can be computed.
Outline of Task
An outline of what we need to do is listed below:
- What we will do is add a C code function to the motor control that generates a random 1 or 0 at the sample rate of the velocity loop, Ts = 1msec. The function is uint16_t prbs( Prbs_t ).
- The random output is scaled and offset to zero mean to generate a signal (int16_t debug3) to add to the loop error signal. This signal is the excitation u(k) we spoke of in Part I.
- Every sample period we also sample the velocity feedback y(k). A time stamp, j, is also generated so that when we log the results we can be assured no values were lost.
- The values {j, u, y} are logged to a UART terminal output while the motor is running.
- We capture the data {j, u, y} from the terminal and use that data in our Matlab script to do the system ID calculation and display the bode plot.
PRBS15
The Pseudo Random Binary Sequence used herein is 15-bits in length and given by the polynomial
PRBS15= x^15 + x^14 + 1
The C code for the PRBS is at the following gist, along with the data structure used:
| Prbs_t is a struct used in the prbs function: | |
| typedef struct | |
| { | |
| uint16_t start; // start LFSR value | |
| uint16_t lfsr; // LFSR current register | |
| uint16_t index; // LFSR index | |
| uint16_t newbit; // current newbit | |
| } Prbs_t; | |
| /** | |
| * @brief generate PRBS15 signal for bode plot system ID | |
| * polynomial x^15 + x^14 + 1 | |
| * @param Prbs_t *p | |
| * @retval int16_t prbs value | |
| */ | |
| uint16_t prbs(Prbs_t *p) | |
| { | |
| p->newbit = (((p->lfsr >> 14) ^ (p->lfsr >> 13)) & 1); | |
| p->lfsr = ((p->lfsr << 1) | p->newbit) & 0x7fff; | |
| p->index++; | |
| if (p->index > (1<<15)-1) | |
| p->index = 0; // ? 0 or 1? | |
| return p->newbit; | |
| } |
The figure below shows a time slice of the PRBS15. It is scaled to amplitude values +/-1. This data was captured from the UART log file and plotted.
Figure 2 - MCU PRBS15 output
STM32 Code
The code added to the STM32 project is shown in the following gist. This is an excerpt of the file speed_torq_ctrl.c.
| /* Run the speed control loop ================================ */ | |
| /* this code part of speed_torq_cntrl.c */ | |
| /* Compute speed error */ | |
| hTargetSpeed = ( int16_t )( wCurrentReference / 65536 ); | |
| // Uses Moving Average Filter updated every PWM period | |
| // and averaged for xxxx samples | |
| hMeasuredSpeed = SPD_GetAvrgMecSpeedUnit( pHandle->SPD ); | |
| /* system ID */ | |
| #define SYS_ID | |
| #ifdef SYS_ID | |
| if(j_indx++>3) | |
| { | |
| j_indx = 0; | |
| debug3 = prbs(p_prbs); | |
| excitation = 2*5*debug3-5; | |
| log_data(excitation, hMeasuredSpeed , (p_prbs->index)); | |
| } | |
| hError = hTargetSpeed - hMeasuredSpeed + excitation; | |
| #else | |
| hError = hTargetSpeed - hMeasuredSpeed; | |
| #endif | |
| hTorqueReference = PI_Controller( pHandle->PISpeed, ( int32_t )hError ); | |
| pHandle->SpeedRefUnitExt = wCurrentReference; | |
| pHandle->TorqueRef = ( int32_t )hTorqueReference * 65536; |
The prbs function is called at line 16 and assigned to variable debug3. At line 17 debug3 is offset to mean 0 and scaled to +/-5 counts and assigned to variable excitation. The excitation is added to the target speed at line 21 and used to compute the error signal. The exitation and feedback velocity are output to the UART at line 19 by calling log_data.
The data logged by the MCU is shown in this gist link for prbs_data9.m. The raw txt file was edited to be a Matlab m-file that run before calling the processing file prbs_scr.m.
MATLAB Code
The Matlab code to compute the system ID is shown in the gist below in file prbs_scr.m.
| %% prbs.m -- analysis of bode using u(k) and y(k) | |
| % load data, should be >> 2^15 values so transient dies out | |
| % btremaine 2022 | |
| % run m file to load data (prbs_data7, 8, 9 | |
| prbs_data9 | |
| % grab last 32767 data points | |
| N= length(U); | |
| y= U(N-32766:N,2); | |
| u= U(N-32766:N,1); | |
| Ts= 4*(1E-3); | |
| w = logspace(-1, log10((2*pi/Ts)), 800); % freq spectrum for bode | |
| %% remove mean from u(k)and y(k)then analyze spectrum | |
| u= u-mean(u); | |
| y= y-mean(y); | |
| figure(1); stairs(y); title('yk response') | |
| figure(2); stairs(u); title('uk response'); axis([0 length(u) 2*min(u) 2*max(u)]); | |
| Ruu= xcorr(u,u); | |
| figure(3); stairs(Ruu); title('xccor uk, uk') | |
| %% Show xcorr x & y | |
| Ryu= xcorr(u,y); | |
| figure(4); stairs(Ryu); title('xccor xk, yk') | |
| %% analyze spectrum | |
| z = iddata(y,u,Ts); | |
| [mag_id, pha_id]= bode(spa(z,500,w),w); | |
| figure(6); clf; | |
| subplot(211); semilogx(w, db(mag_id(:,:))); title('Sys ID Bode'); ylabel('Mag (dB)'); | |
| xlabel('Freq (rad/sec)'); axis([min(w) max(w) -30 5 ]) | |
| subplot(212); semilogx(w, pha_id(:,:) ); | |
| ylabel('Phase (deg)'); xlabel('Freq (rad/sec)'); axis([min(w) max(w) -180 90]) |
This m-file plots out a number of figures I show below.
The first file shown is the u(k) collected from the MCU along with the y(k) collected.
               Figure 3 - u(k)                           Figure 4 - y(k)
The next files show the cross correlation Ruu and Rux,
              Figure 5 - Ruu                      Figure 6 - Ruy
    
The plot for Ruu is an impule response indicating the spectrum of u(k) is random white noise. The plot of Ruy is the impulse response of the closed loop system from u(k) to y(k). We use the Matlab function spa with u(k) and y(k) at lines 31 and 32Â to compute the Bode plot shown in figure 7.
Figure 7 - Experimental Bode plot
As expected for a closed velocity loop, the gain at DC is unity and the phase shift is 0 deg. The gain rolls off -3db at about 9 rad/sec (1.4Hz). Below the Nyquist frequency of 500Hz there is no significant amplitude peaking evident.
Revisit Block Diagram
Below I revist the simple block diagram shown in figure 1. Here I use the block diagram of the STM32 FOC code and show where we inject u(k) and retrieve y(k). All the detail of the cascaded FOC loops is embedded in the closed loop transfer function from u(k) to y(k).
Figure 8 - FOC Block diagram with System ID Test Points u(k), y(k)
If I wanted to close the velocity loop within a phase loop I would use Matlab to generate a Bode plot with arbitrary poles and zeros to duplicate the experimental plot in figure 7 then cascade the outer phase loop with the empirical transfer function to look at stability.
Conclusion
I have shown how system ID can be used to measure the Bode plot of a velocity loop comprised of an FOC controller implemented on an STM32 M4 Cortex. The additions to the STM code are shown and the Matlab m-file for processing the data is given.