View source for AXY:Element Coordination/Multi Dimensional Tracking Algorithm (Orientation)

= Multi-Dimensional Tracking Algorithm = 

This algorithm is used for multi-dimensional '''robot based''' MF tracking.<br>
In this document position-tracking is presented.<br>
Orientation  is considered to be a 3-dimensional vector with its derivatives (velocity, acceleration, jerk): <math>\vec e </math>  , <math>\vec de</math> , <math>\vec dde</math><br>

<math>\vec e  = (yaw,pitch,roll)</math><br>

The algorithm consist of a state machine with the following states:<br>

# Tracking process
# Full Synchronization follow the master
# Stopping (de-synchronization process)

== State: Full Synchronization follow the master ==


* Check if master velocity or acceleration exceeds max master frame's values if yes return an error

* <math>\overrightarrow {tack} := \overrightarrow {master}</math>  ''copying all derivatives''

* ''reset:'' '''flopstime = flops = 0'''

== State: Tracking process ==

=== Initial Setup and testing ===
<table style="float:right" border = "1">
<tr><td>
Value of [[MC-Basic:movingFrame.ACCELERATIONMAXROT|AccMaxRot]] is used of checking of acceleration limit.
</td></tr>
<tr><td>
Value of [[Axystems:MC-Basic:movingFrame.VELOCITYMAXROT|VelMaxRot]] is used of checking of velocity limit.
</td></tr>
</table>

* Check if master velocity or acceleration exceeds max master frame's values if yes return an error<br>

* Compute differences:<br>
<math>\overrightarrow {ori}_{diff} = \overrightarrow {master}_{ori} - \overrightarrow {track}_{ori}</math>  &nbsp;&nbsp;  ''pure delta-orientation''<br>
<math>\overrightarrow {vel}_{diff} = \overrightarrow {master}_{vel} - \overrightarrow {track}_{vel}</math>  &nbsp;&nbsp;  ''pure delta-velocity''<br>

=== Time-to-Reach initial estimation ===

<table style="float:right" border = "1">
<tr><td>
<math>{sync_{jrk}}</math> defined as   [[Axystems:MC-Basic:robot.JERKSYNCROT|SyncJerkRot]]
</td></tr>
<tr><td>
<math>{sync_{acc}}</math> defined as   [[Axystems:MC-Basic:robot.ACCELERATIONSYNCROT|SyncAcclerationRot]]
</td></tr>
</table>


''This is an estimation of time needed to reach the target. It is not an exact formula<br>
''if the slave velocity is lower then master's we need more time: once to get to a higher velocity then the masters and once <br>
''to get from that (higher) velocity to the masters - therefore the multiplication by 2''<br>

<br>
<math>time_{toreach} = \sqrt{\frac {K*|{\overrightarrow {vel}_{diff}}|} {sync_{jrk}} }  + \sqrt{\frac{4*|{\overrightarrow {ori}_{diff}}|}{sync_{acc}}}</math><br>
<br>

''where'' <math> K = \begin{cases} 4,  & \mbox{if }|\overrightarrow  {master}_{vel}| > |\overrightarrow {track}_{vel}| \\2, & \mbox{if }| \overrightarrow {master}_{vel}| \le |\overrightarrow {track}_{vel}| \end{cases}</math> <br>


<table style="float:right" border = "1">
<tr><td>
DampingFactor -  user defined by: [[Axystems:MC-Basic:movingFrame.DAMPINGFACTOR|MF.DAMPINGFACTOR]]
</td></tr>
</table>

''Additionaly we mutliply by a prediction factor - to decrease acc/jerk<br>
(rounding to integer number of samples):''

<pre>
time_to_reach *= DampingFactor
time_to_reach = T*int(time_to_reach/T+1) 
</pre>

=== Prediction of the Rendezvous-Point ===
''The predicted orientation of the rendezvous (minus T, if it will happen in this sample!)''<br>
<math>\overrightarrow {pred}_{ori}</math>:&nbsp; ''orientation prediction assuming constant acceleration''<br>
<math>\overrightarrow {pred}_{diff}</math>:&nbsp;''Predicted orientation difference''<br>
<math>\overrightarrow {pred}_{vel}</math>:&nbsp;'' velocity prediction''<br>


<math>t = (time_{toreach}-T)</math><br>
<math>\overrightarrow {pred}_{ori}  = \overrightarrow {master}_{ori} + t*\overrightarrow {master}_{vel} + t^2*\overrightarrow {master}_{acc}/2</math><br>                   
<math>\overrightarrow {pred}_{diff} = \overrightarrow {pred}_{ori} - \overrightarrow {track}_{ori}</math><br>		
<math>\overrightarrow {pred}_{vel}  = \overrightarrow {master}_{vel} + t*\overrightarrow {master}_{acc}</math><br>	



'' Assuming the acceleration will be zero:''<br>
<math>\overrightarrow {pred}_{acc} = 0</math><br>		
																			  

'' Polynomial coefficients of 5-degree polynom connecting the current p,v,a to the randevous p,v,a''<br>
<math>\overrightarrow a0 = \overrightarrow {track}_{ori}	</math>		 - ''not used, directly written into code''<br>
<math>\overrightarrow a1 = \overrightarrow {track}_{vel}</math>			 - ''not used, directly written into code''<br>
<math>\overrightarrow a2 = 0.5*\overrightarrow {track}_{acc}</math>		 - ''not used, directly written into code''<br>
<math>\overrightarrow a3 = (0.5*(\overrightarrow {pred}_{acc}-3*\overrightarrow {track}_{acc})~+~(10*\overrightarrow {pred}_{diff}/time_{toreach}</math><math>-~6*\overrightarrow {track}_{vel}~-~4*\overrightarrow {pred}_{vel})/time_{toreach})/time_{toreach}</math><br>
<math>\overrightarrow a4 = (((5*\overrightarrow {pred}_{diff}/time_{toreach}~-~(\overrightarrow {pred}_{vel}+4*track_{vel}))/time_{toreach}</math><math>- 1.5*\overrightarrow {track}_{acc})/time_{toreach}~-~2*\overrightarrow a3)/time_{toreach}</math><br>
<math>\overrightarrow a5 = ((((\overrightarrow {pred}_{diff}/time_{toreach}~-~\overrightarrow {track}_{vel})/time_{toreach}</math><math>-0.5*\overrightarrow {track}_{acc})/time_{toreach}~-~\overrightarrow a3)/time_{toreach}~-~\overrightarrow a4)/time_{toreach}</math><br>

=== Next Sample Orinetation ===
''The orinetation  at the next sample will be:''<br>
''Initial increment  (full displacement):&nbsp;''<math>\overrightarrow {dp}     = \overrightarrow a5*T^5 + \overrightarrow a4*T^4 + \overrightarrow a3*T^3 + 0.5*\overrightarrow {track}_{acc}*T^2 + \overrightarrow {track}_{vel}*T</math><br>

'' The accleration and jerk increments could be computed in this way also.''<br>
'' But they are computed as average values for the time of sample duration, this approach does fit more to ''<br>
'' the tools we are using to test this feature, as they all compute acceleration & jerk by numeric differentiation''<br>
'' inside one sample.''<br>
'' The side effect of all this is that we can have a change in jerk/acc sign inside one sample and the average value we are using ''<br>
'' ddp,dddp could have an opposite sign, so we are seeing effects like jerk or acceleration is limited from a "wrong side" i.e. instead''<br>
'' using jmax -jmax is used. There is not much we can do against it, using exact jerk/acc values (from polynom) returns than wrong average values ''<br>
'' we observe as jerk/acc exceed over theri max values. So the average value approach is not perfect but gives results within the given limits.''<br>


'' Initial setting of predicted time and the filter limit flag (if the filter did work - this flag will be on)''
<pre>
limit        = false 
state.satVel = false 
state.satAcc = false  
state.satJrk = false 
</pre>

=== Velocity, Acceleration and Jerk Filter ===

<table style="float:right" border = "1">
<tr><td>
sync_vel defined as  [[Axystems:MC-Basic:robot.VELOCITYSYNCROT|VelSyncRot]]
</td></tr>
<tr><td>
sync_acc defined as  [[Axystems:MC-Basic:robot.ACCELERATIONSYNCROT|AccSyncRot]]
</td></tr>
<tr><td>
sync_jrk defined as  [[Axystems:MC-Basic:robot.JERKSYNCROT|JrkSyncRot]]
</td></tr>
<tr><td>
T is sampling time
</td></tr>

</table>


<math> \overrightarrow {de} = \begin{cases} sync_{velT} \cdot \overrightarrow {de^*} ,  & \mbox{if } |\overrightarrow {de}| > sync_{velT} \\ limit = true \\ \overrightarrow {de} ,  & \mbox{if } |\overrightarrow {de}| \le sync_{velT}\end{cases}</math> 


where <math> \vec a^* = \vec a /|\vec a| </math> i.e. unit vector

==== Setting Initial velocity delta====	
<math>\overrightarrow {dde}  = \overrightarrow {de} - \overrightarrow {de_0}</math> <br>


<math> \overrightarrow {dde} = \begin{cases} sync_{acc}T^2\cdot \overrightarrow {dde^*} ,  & \mbox{if } |\overrightarrow {dde}| > sync_{acc}T^2 \\ \overrightarrow {de} = sync_{acc}T^2\cdot \overrightarrow {dde^*} + \overrightarrow {de_0} \\limit = true \\ \overrightarrow {dde},
 & \mbox{if }|\overrightarrow {dde}| \le sync_{acc}T^2 \end{cases}</math>

==== Setting Initial acceleration delta====	
<math>\overrightarrow {ddde}  = \overrightarrow {dde} - \overrightarrow {dde_0}</math> <br>


<math> \overrightarrow {ddde} = \begin{cases} sync_{jrk}T^3\cdot \overrightarrow {ddde^*} ,  & \mbox{if } |\overrightarrow {ddde}| > sync_{jrk}T3 \\ \overrightarrow {dde} = sync_{jrk}T^3 \cdot \overrightarrow {ddde^*}  + \overrightarrow {dde_0} \\\overrightarrow {de} = \overrightarrow {dde} + \overrightarrow {de_0}\\limit = true \\ \overrightarrow {ddde},
 & \mbox{if }|\overrightarrow {ddde}| \le sync_{jrk}T^3 \end{cases}</math>

==== New orientation is: ====	
<math>track_{ori}  = track_{ori} + de</math>

==== If the filter was activated then: ====
'' if the velocity,accleration or jerk is limited ('''limit = true''') then the velocity computed using polynomials does''<br>
<math> \overrightarrow {track}_{vel}   = \overrightarrow {de}/T  </math><br>
<math> \overrightarrow {track}_{acc}   = \overrightarrow {dde}/T^2</math><br>

<br>
'' Velocity & Acceleration will be computed exactly not using de and dde, it has been shown that this''
'' approach is much better - means more stable''<br>

<math> \overrightarrow {track}_{vel}   = 5*\overrightarrow a5*T^4  + 4*\overrightarrow a4*T^3  + 3*\overrightarrow a3*T^2  + \overrightarrow {track}_{acc}*T + \overrightarrow {track}_{vel}</math><br>
<math> \overrightarrow {track}_{acc}   = 20*\overrightarrow a5*T^3 + 12*\overrightarrow a4*T^2  + 6*\overrightarrow a3*T   + \overrightarrow {track}_{acc}</math><br>

====keep the old values for next sample====
<math>\overrightarrow {dde}_0 = \overrightarrow {dde}</math> <br>
<math>\overrightarrow {de}_0  = \overrightarrow {de}</math> <br>

====System is synchronized if NEW delta's are under certesian thresholds:====
<math>\overrightarrow {dv}_0 = \overrightarrow {dv}</math>; - ''keep the previous dv''<br>
<math>dv = \overrightarrow {master}_{vel} - \overrightarrow {track}_{vel}</math><br>


<table style="float:right" border = "1">
<tr><td>
syncGain defined by the user with: [[Axystems:MC-Basic:movingFrame.FILTERFACTOR|MF.FilterFactor]]
</td></tr>
</table>



==== Successful tracking completion ====

'''if''' &nbsp; <math>(|\overrightarrow {dv}| \le syncGain * sync_{acc} T)</math> &nbsp;'''and''' &nbsp;<math>(|\overrightarrow {master}_{ori} - \overrightarrow {track}_{ori}| \le syncGain * sync_{acc} T^2)</math> <br>
'' Synchronization'' <br>
'' Take the middle for an additional smoothing:''<br>
<math>\overrightarrow {track}_{ori}	   =	0.5*(\overrightarrow {track}_{ori} + \overrightarrow {master}_{ori})</math><br>
<math>\overrightarrow {track}_{vel}	   =	0.5*(\overrightarrow {track}_{vel} + \overrightarrow {master}_{vel})</math><br>
<pre>
ResetFilter();		// Reset the filter
state.tracking     =    MOT_MASTER_SYNC; // System synchronized<br>
</pre>
'''else if( dv0*dv < 0)'''<br>
'' Check against over-oscillation ''<br>
<pre>
if (flops == -1) flops = 0; // ignore the first sample
else	         flops++;
flopstime = 0;
</pre>

==== Over-Oscillation Check ====

<table style="float:right" border = "1">
<tr><td>
maxFlops defined by the user with: [[Axystems:MC-Basic:movingFrame.MAXFLOPS|MF.MaxFlops]]
</td></tr>
</table>


'''<u>if (flops > maxFlops && (flopstime > maxFlopsTime && fabs(dv) > syncGain * sync_accT2))</u>'''<br>
<pre>
state.tracking 	= MOT_MASTER_OSC; // OScilationss
</pre>
'''else'''
<pre>
flopstime++;
</pre>

== State: Stopping (de-synchronization process) ==

<pre>
flopstime = flops = 0;
</pre>

* De-Syncing profile, follow the given profile, ignore the real source


* determine the curvature parameters path:


<math>\overrightarrow {desync}_{start} </math> , <math>\overrightarrow {desync}_{normal} </math>, <math>\overrightarrow {desync}_{center} </math><br>

or linear movement:

<math>\overrightarrow {desync}_{direction} </math> 


* initialize stopping profile with current values of curvature angle:

desync.pos = <math>\alpha </math><br>
desync.vel = <math>\dot \alpha </math><br>
desync.acc = <math>\ddot \alpha </math><br>

See: [[AXY:Element_Coordination/Multi_Dimensional_Tracking_Algorithm_(Position)/Theoretical_Background|Theoretical_Background]]


* initialize the profiler for stopping - computing the stopping (deceleration path) ad set as new target position for the profiler.

* during stopping:

<pre>
desync.Profile();
</pre>


<math>\overrightarrow p = \overrightarrow {desync}_{start} \cdot cos(\alpha(t))  + \overrightarrow {desync}_{normal} \cdot sin(\alpha(t))  + \overrightarrow {desync}_{center} </math><br>

or in case of linear motion:

<math>\overrightarrow p = \overrightarrow {desync}_{direction} \cdot l(t) + \overrightarrow {track}_{pos} </math>


<pre>
if(desync.path.Status.stop)			// if stopped,change the state to OFF
	 SetState(MOT_MASTER_OFF);
</pre>

[[Category:Axystems:Motion:MovingFrame]]