Algorithm: The Algorithm for SSMA Calculation

//The data acquisition algorithm is used to extract all sensor data; Generate Date List, Status list; Get all the date points $p_i\left(g,a,m,q\right)$, and put them in the Date List; Get all the status values $a_i$, and put them in the Status list; $K=\left(p_1, p_2,p_3,p_4\right)$; $i=1, 2, 3, 4$; For j = 0 to Date List.num do   Start the timer;   Send $p_i$ to; If $i=4$, wait until the time end and get back; end If; Update the $K$; End //The calculation for SSMA Generate Date List, Status list; Get all the date points $K$, and put them in the Date List; Get the status value $K_0$ in $K$, and put it in the Status list; Put the default status value $m$ in the Status list; If $K_0=m$, decode the $p_i$, and save it to the queue, save the initial data $K$; If $i=1, 2$, get $p_i$ to calculate the angel of movable arm, and save it to Date List; If $i=2, 3$, get $p_i$ to calculate the steering angel, and save it to Date List; If $i=3, 4$, get $p_i$ to calculate the swing angel, and save it to Date List;  Get all them out including $p_3$ to calculate SSMA, and save it to Date List; End If Update the$K$, Update SSMA; End.