AXY:Element Coordination/Multi Dimensional Tracking Algorithm (Position)

Multi Dimensional Tracking Algorithm

This algorithm is used for multi-dimensional robot based MF tracking.
In this document position-tracking is presented.
Position is considered to be a 3-dimensional vector with its derivatives (velocity, acceleration, jerk): Failed to parse (MathML with SVG or PNG fallback (recommended for modern browsers and accessibility tools): Invalid response ("Math extension cannot connect to Restbase.") from server "https://en.wikipedia.org/api/rest_v1/":): {\displaystyle \vec p } , Failed to parse (MathML with SVG or PNG fallback (recommended for modern browsers and accessibility tools): Invalid response ("Math extension cannot connect to Restbase.") from server "https://en.wikipedia.org/api/rest_v1/":): {\displaystyle \vec v} , Failed to parse (MathML with SVG or PNG fallback (recommended for modern browsers and accessibility tools): Invalid response ("Math extension cannot connect to Restbase.") from server "https://en.wikipedia.org/api/rest_v1/":): {\displaystyle \vec a}

Failed to parse (MathML with SVG or PNG fallback (recommended for modern browsers and accessibility tools): Invalid response ("Math extension cannot connect to Restbase.") from server "https://en.wikipedia.org/api/rest_v1/":): {\displaystyle \vec p = (x,y,z)}

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

1. Tracking process
2. Full Synchronization follow the master
3. 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

• Failed to parse (MathML with SVG or PNG fallback (recommended for modern browsers and accessibility tools): Invalid response ("Math extension cannot connect to Restbase.") from server "https://en.wikipedia.org/api/rest_v1/":): {\displaystyle \overrightarrow {tack} := \overrightarrow {master}} copying all derivatives

• reset: flopstime = flops = 0

State: Tracking process

Initial Setup and testing

Value of AccMaxTran is used of checking of acceleration limit.

Value of VelMaxTran is used of checking of velocity limit.

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

• Compute differences:
  

Failed to parse (MathML with SVG or PNG fallback (recommended for modern browsers and accessibility tools): Invalid response ("Math extension cannot connect to Restbase.") from server "https://en.wikipedia.org/api/rest_v1/":): {\displaystyle \overrightarrow {pos}_{diff} = \overrightarrow {master}_{pos} - \overrightarrow {track}_{pos}}    pure delta-position
Failed to parse (MathML with SVG or PNG fallback (recommended for modern browsers and accessibility tools): Invalid response ("Math extension cannot connect to Restbase.") from server "https://en.wikipedia.org/api/rest_v1/":): {\displaystyle \overrightarrow {vel}_{diff} = \overrightarrow {master}_{vel} - \overrightarrow {track}_{vel}}    pure delta-velocity

Time-to-Reach initial estimation

Failed to parse (MathML with SVG or PNG fallback (recommended for modern browsers and accessibility tools): Invalid response ("Math extension cannot connect to Restbase.") from server "https://en.wikipedia.org/api/rest_v1/":): {\displaystyle {sync_{jrk}}} defined as SyncJerkTran

Failed to parse (MathML with SVG or PNG fallback (recommended for modern browsers and accessibility tools): Invalid response ("Math extension cannot connect to Restbase.") from server "https://en.wikipedia.org/api/rest_v1/":): {\displaystyle {sync_{acc}}} defined as SyncAcclerationTran

This is an estimation of time needed to reach the target. It is not an exact formula
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
to get from that (higher) velocity to the masters - therefore the multiplication by 2

Failed to parse (MathML with SVG or PNG fallback (recommended for modern browsers and accessibility tools): Invalid response ("Math extension cannot connect to Restbase.") from server "https://en.wikipedia.org/api/rest_v1/":): {\displaystyle time_{toreach} = \sqrt{\frac {K*|{\overrightarrow {vel}_{diff}}|} {sync_{jrk}} } + \sqrt{\frac{4*|{\overrightarrow {pos}_{diff}}|}{sync_{acc}}}}

where Failed to parse (MathML with SVG or PNG fallback (recommended for modern browsers and accessibility tools): Invalid response ("Math extension cannot connect to Restbase.") from server "https://en.wikipedia.org/api/rest_v1/":): {\displaystyle K = \begin{cases} 4, & \mbox{if }|\overrightarrow {master}_{vel}| > |\overrightarrow {track}_{vel}| \\2, & \mbox{if }| \overrightarrow {master}_{vel}| \le |\overrightarrow {track}_{vel}| \end{cases}}

DampingFactor - user defined by: MF.DAMPINGFACTOR

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

time_to_reach *= DampingFactor
time_to_reach = T*int(time_to_reach/T+1) 

Prediction of the Rendezvous-Point

The predicted position of the rendezvous (minus T, if it will happen in this sample!)
Failed to parse (MathML with SVG or PNG fallback (recommended for modern browsers and accessibility tools): Invalid response ("Math extension cannot connect to Restbase.") from server "https://en.wikipedia.org/api/rest_v1/":): {\displaystyle \overrightarrow {pred}_{pos}} :  position prediction assuming constant acceleration
Failed to parse (MathML with SVG or PNG fallback (recommended for modern browsers and accessibility tools): Invalid response ("Math extension cannot connect to Restbase.") from server "https://en.wikipedia.org/api/rest_v1/":): {\displaystyle \overrightarrow {pred}_{diff}} : Predicted position difference
Failed to parse (MathML with SVG or PNG fallback (recommended for modern browsers and accessibility tools): Invalid response ("Math extension cannot connect to Restbase.") from server "https://en.wikipedia.org/api/rest_v1/":): {\displaystyle \overrightarrow {pred}_{vel}} :  velocity prediction

Failed to parse (MathML with SVG or PNG fallback (recommended for modern browsers and accessibility tools): Invalid response ("Math extension cannot connect to Restbase.") from server "https://en.wikipedia.org/api/rest_v1/":): {\displaystyle t = (time_{toreach}-T)}
Failed to parse (MathML with SVG or PNG fallback (recommended for modern browsers and accessibility tools): Invalid response ("Math extension cannot connect to Restbase.") from server "https://en.wikipedia.org/api/rest_v1/":): {\displaystyle \overrightarrow {pred}_{pos} = \overrightarrow {master}_{pos} + t*\overrightarrow {master}_{vel} + t^2*\overrightarrow {master}_{acc}/2}
Failed to parse (MathML with SVG or PNG fallback (recommended for modern browsers and accessibility tools): Invalid response ("Math extension cannot connect to Restbase.") from server "https://en.wikipedia.org/api/rest_v1/":): {\displaystyle \overrightarrow {pred}_{diff} = \overrightarrow {pred}_{pos} - \overrightarrow {track}_{pos}}
Failed to parse (MathML with SVG or PNG fallback (recommended for modern browsers and accessibility tools): Invalid response ("Math extension cannot connect to Restbase.") from server "https://en.wikipedia.org/api/rest_v1/":): {\displaystyle \overrightarrow {pred}_{vel} = \overrightarrow {master}_{vel} + t*\overrightarrow {master}_{acc}}

Assuming the acceleration will be zero:
Failed to parse (MathML with SVG or PNG fallback (recommended for modern browsers and accessibility tools): Invalid response ("Math extension cannot connect to Restbase.") from server "https://en.wikipedia.org/api/rest_v1/":): {\displaystyle \overrightarrow {pred}_{acc} = 0}

Polynomial coefficients of 5-degree polynom connecting the current p,v,a to the randevous p,v,a
Failed to parse (MathML with SVG or PNG fallback (recommended for modern browsers and accessibility tools): Invalid response ("Math extension cannot connect to Restbase.") from server "https://en.wikipedia.org/api/rest_v1/":): {\displaystyle \overrightarrow a0 = \overrightarrow {track}_{pos} } - not used, directly written into code
Failed to parse (MathML with SVG or PNG fallback (recommended for modern browsers and accessibility tools): Invalid response ("Math extension cannot connect to Restbase.") from server "https://en.wikipedia.org/api/rest_v1/":): {\displaystyle \overrightarrow a1 = \overrightarrow {track}_{vel}} - not used, directly written into code
Failed to parse (MathML with SVG or PNG fallback (recommended for modern browsers and accessibility tools): Invalid response ("Math extension cannot connect to Restbase.") from server "https://en.wikipedia.org/api/rest_v1/":): {\displaystyle \overrightarrow a2 = 0.5*\overrightarrow {track}_{acc}} - not used, directly written into code
Failed to parse (MathML with SVG or PNG fallback (recommended for modern browsers and accessibility tools): Invalid response ("Math extension cannot connect to Restbase.") from server "https://en.wikipedia.org/api/rest_v1/":): {\displaystyle \overrightarrow a3 = (0.5*(\overrightarrow {pred}_{acc}-3*\overrightarrow {track}_{acc})~+~(10*\overrightarrow {pred}_{diff}/time_{toreach}} Failed to parse (MathML with SVG or PNG fallback (recommended for modern browsers and accessibility tools): Invalid response ("Math extension cannot connect to Restbase.") from server "https://en.wikipedia.org/api/rest_v1/":): {\displaystyle -~6*\overrightarrow {track}_{vel}~-~4*\overrightarrow {pred}_{vel})/time_{toreach})/time_{toreach}}
Failed to parse (MathML with SVG or PNG fallback (recommended for modern browsers and accessibility tools): Invalid response ("Math extension cannot connect to Restbase.") from server "https://en.wikipedia.org/api/rest_v1/":): {\displaystyle \overrightarrow a4 = (((5*\overrightarrow {pred}_{diff}/time_{toreach}~-~(\overrightarrow {pred}_{vel}+4*track_{vel}))/time_{toreach}} Failed to parse (MathML with SVG or PNG fallback (recommended for modern browsers and accessibility tools): Invalid response ("Math extension cannot connect to Restbase.") from server "https://en.wikipedia.org/api/rest_v1/":): {\displaystyle - 1.5*\overrightarrow {track}_{acc})/time_{toreach}~-~2*\overrightarrow a3)/time_{toreach}}
Failed to parse (MathML with SVG or PNG fallback (recommended for modern browsers and accessibility tools): Invalid response ("Math extension cannot connect to Restbase.") from server "https://en.wikipedia.org/api/rest_v1/":): {\displaystyle \overrightarrow a5 = ((((\overrightarrow {pred}_{diff}/time_{toreach}~-~\overrightarrow {track}_{vel})/time_{toreach}} Failed to parse (MathML with SVG or PNG fallback (recommended for modern browsers and accessibility tools): Invalid response ("Math extension cannot connect to Restbase.") from server "https://en.wikipedia.org/api/rest_v1/":): {\displaystyle -0.5*\overrightarrow {track}_{acc})/time_{toreach}~-~\overrightarrow a3)/time_{toreach}~-~\overrightarrow a4)/time_{toreach}}

Next Sample Position

The position at the next sample will be:
Initial increment (full displacement): Failed to parse (MathML with SVG or PNG fallback (recommended for modern browsers and accessibility tools): Invalid response ("Math extension cannot connect to Restbase.") from server "https://en.wikipedia.org/api/rest_v1/":): {\displaystyle \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}

The acceleration and jerk increments could be computed in this way also.
But they are computed as average values for the time of sample duration, this approach does fit more to
the tools we are using to test this feature, as they all compute acceleration & jerk by numeric differentiation
inside one sample.
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
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
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
we observe as jerk/acc exceed over their max values. So the average value approach is not perfect but gives results within the given limits.

Initial setting of predicted time and the filter limit flag (if the filter did work - this flag will be on)

limit        = false 
state.satVel = false 
state.satAcc = false  
state.satJrk = false 

Velocity, Acceleration and Jerk Filter

sync_vel defined as VelSyncTran

sync_acc defined as AccSyncTran

sync_jrk defined as JrkSyncTran

T is sampling time

Failed to parse (MathML with SVG or PNG fallback (recommended for modern browsers and accessibility tools): Invalid response ("Math extension cannot connect to Restbase.") from server "https://en.wikipedia.org/api/rest_v1/":): {\displaystyle \overrightarrow {dp} = \begin{cases} sync_{velT} \cdot \overrightarrow {dp^*} , & \mbox{if } |\overrightarrow {dp}| > sync_{velT} \\ limit = true \\ \overrightarrow {dp} , & \mbox{if } |\overrightarrow {dp}| \le sync_{velT}\end{cases}}

where Failed to parse (MathML with SVG or PNG fallback (recommended for modern browsers and accessibility tools): Invalid response ("Math extension cannot connect to Restbase.") from server "https://en.wikipedia.org/api/rest_v1/":): {\displaystyle \vec a^* = \vec a /|\vec a| } i.e. unit vector

Setting Initial velocity delta

Failed to parse (MathML with SVG or PNG fallback (recommended for modern browsers and accessibility tools): Invalid response ("Math extension cannot connect to Restbase.") from server "https://en.wikipedia.org/api/rest_v1/":): {\displaystyle \overrightarrow {ddp} = \overrightarrow {dp} - \overrightarrow {dp_0}}

Failed to parse (MathML with SVG or PNG fallback (recommended for modern browsers and accessibility tools): Invalid response ("Math extension cannot connect to Restbase.") from server "https://en.wikipedia.org/api/rest_v1/":): {\displaystyle \overrightarrow {ddp} = \begin{cases} sync_{acc}T^2\cdot \overrightarrow {ddp^*} , & \mbox{if } |\overrightarrow {ddp}| > sync_{acc}T^2 \\ \overrightarrow {dp} = sync_{acc}T^2\cdot \overrightarrow {ddp^*} + \overrightarrow {dp_0} \\limit = true \\ \overrightarrow {ddp}, & \mbox{if }|\overrightarrow {ddp}| \le sync_{acc}T^2 \end{cases}}

Setting Initial acceleration delta

Failed to parse (MathML with SVG or PNG fallback (recommended for modern browsers and accessibility tools): Invalid response ("Math extension cannot connect to Restbase.") from server "https://en.wikipedia.org/api/rest_v1/":): {\displaystyle \overrightarrow {dddp} = \overrightarrow {ddp} - \overrightarrow {ddp_0}}

Failed to parse (MathML with SVG or PNG fallback (recommended for modern browsers and accessibility tools): Invalid response ("Math extension cannot connect to Restbase.") from server "https://en.wikipedia.org/api/rest_v1/":): {\displaystyle \overrightarrow {dddp} = \begin{cases} sync_{jrk}T^3\cdot \overrightarrow {dddp^*} , & \mbox{if } |\overrightarrow {dddp}| > sync_{jrk}T3 \\ \overrightarrow {ddp} = sync_{jrk}T^3 \cdot \overrightarrow {dddp^*} + \overrightarrow {ddp_0} \\\overrightarrow {dp} = \overrightarrow {ddp} + \overrightarrow {dp_0}\\limit = true \\ \overrightarrow {dddp}, & \mbox{if }|\overrightarrow {dddp}| \le sync_{jrk}T^3 \end{cases}}

New position is:

Failed to parse (MathML with SVG or PNG fallback (recommended for modern browsers and accessibility tools): Invalid response ("Math extension cannot connect to Restbase.") from server "https://en.wikipedia.org/api/rest_v1/":): {\displaystyle track_{pos} = track_{pos} + dp}

If the filter was activated then:

if the velocity,accleration or jerk is limited (limit = true) then the velocity computed using polynomials does
Failed to parse (MathML with SVG or PNG fallback (recommended for modern browsers and accessibility tools): Invalid response ("Math extension cannot connect to Restbase.") from server "https://en.wikipedia.org/api/rest_v1/":): {\displaystyle \overrightarrow {track}_{vel} = \overrightarrow {dp}/T }
Failed to parse (MathML with SVG or PNG fallback (recommended for modern browsers and accessibility tools): Invalid response ("Math extension cannot connect to Restbase.") from server "https://en.wikipedia.org/api/rest_v1/":): {\displaystyle \overrightarrow {track}_{acc} = \overrightarrow {ddp}/T^2}

Velocity & Accleration will be computed exactly not using dp and ddp, it has been shown that this approach is much better - means more stable

Failed to parse (MathML with SVG or PNG fallback (recommended for modern browsers and accessibility tools): Invalid response ("Math extension cannot connect to Restbase.") from server "https://en.wikipedia.org/api/rest_v1/":): {\displaystyle \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}}
Failed to parse (MathML with SVG or PNG fallback (recommended for modern browsers and accessibility tools): Invalid response ("Math extension cannot connect to Restbase.") from server "https://en.wikipedia.org/api/rest_v1/":): {\displaystyle \overrightarrow {track}_{acc} = 20*\overrightarrow a5*T^3 + 12*\overrightarrow a4*T^2 + 6*\overrightarrow a3*T + \overrightarrow {track}_{acc}}

keep the old values for next sample

Failed to parse (MathML with SVG or PNG fallback (recommended for modern browsers and accessibility tools): Invalid response ("Math extension cannot connect to Restbase.") from server "https://en.wikipedia.org/api/rest_v1/":): {\displaystyle \overrightarrow {ddp}_0 = \overrightarrow {ddp}}
Failed to parse (MathML with SVG or PNG fallback (recommended for modern browsers and accessibility tools): Invalid response ("Math extension cannot connect to Restbase.") from server "https://en.wikipedia.org/api/rest_v1/":): {\displaystyle \overrightarrow {dp}_0 = \overrightarrow {dp}}

System is synchronized if NEW delta's are under certesian thresholds:

Failed to parse (MathML with SVG or PNG fallback (recommended for modern browsers and accessibility tools): Invalid response ("Math extension cannot connect to Restbase.") from server "https://en.wikipedia.org/api/rest_v1/":): {\displaystyle \overrightarrow {dv}_0 = \overrightarrow {dv}} ; - keep the previous dv
Failed to parse (MathML with SVG or PNG fallback (recommended for modern browsers and accessibility tools): Invalid response ("Math extension cannot connect to Restbase.") from server "https://en.wikipedia.org/api/rest_v1/":): {\displaystyle dv = \overrightarrow {master}_{vel} - \overrightarrow {track}_{vel}}

syncGain defined by the user with: MF.FilterFactor

Successful tracking completion

if   Failed to parse (MathML with SVG or PNG fallback (recommended for modern browsers and accessibility tools): Invalid response ("Math extension cannot connect to Restbase.") from server "https://en.wikipedia.org/api/rest_v1/":): {\displaystyle (|\overrightarrow {dv}| \le syncGain * sync_{acc} T)}  and  Failed to parse (MathML with SVG or PNG fallback (recommended for modern browsers and accessibility tools): Invalid response ("Math extension cannot connect to Restbase.") from server "https://en.wikipedia.org/api/rest_v1/":): {\displaystyle (|\overrightarrow {master}_{pos} - \overrightarrow {track}_{pos}| \le syncGain * sync_{acc} T^2)}
Synchronization
Take the middle for an additional smoothing:
Failed to parse (MathML with SVG or PNG fallback (recommended for modern browsers and accessibility tools): Invalid response ("Math extension cannot connect to Restbase.") from server "https://en.wikipedia.org/api/rest_v1/":): {\displaystyle \overrightarrow {track}_{pos} = 0.5*(\overrightarrow {track}_{pos} + \overrightarrow {master}_{pos})}
Failed to parse (MathML with SVG or PNG fallback (recommended for modern browsers and accessibility tools): Invalid response ("Math extension cannot connect to Restbase.") from server "https://en.wikipedia.org/api/rest_v1/":): {\displaystyle \overrightarrow {track}_{vel} = 0.5*(\overrightarrow {track}_{vel} + \overrightarrow {master}_{vel})}

ResetFilter();		// Reset the filter
state.tracking     =    MOT_MASTER_SYNC; // System synchronized<br>

else if( dv0*dv < 0)
Check against over-oscillation

if (flops == -1) flops = 0; // ignore the first sample
else	         flops++;
flopstime = 0;

Over-Oscillation Check

maxFlops defined by the user with: MF.MaxFlops

if (flops > maxFlops && (flopstime > maxFlopsTime && fabs(dv) > syncGain * sync_accT2))

state.tracking 	= MOT_MASTER_OSC; // OScilationss

else

flopstime++;

State: Stopping (de-synchronization process)

flopstime = flops = 0;

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

• determine the curvature parameters path:

Failed to parse (MathML with SVG or PNG fallback (recommended for modern browsers and accessibility tools): Invalid response ("Math extension cannot connect to Restbase.") from server "https://en.wikipedia.org/api/rest_v1/":): {\displaystyle \overrightarrow {desync}_{start} } , Failed to parse (MathML with SVG or PNG fallback (recommended for modern browsers and accessibility tools): Invalid response ("Math extension cannot connect to Restbase.") from server "https://en.wikipedia.org/api/rest_v1/":): {\displaystyle \overrightarrow {desync}_{normal} } , ${\overrightarrow {desync}}_{center}$

or linear movement:

Failed to parse (MathML with SVG or PNG fallback (recommended for modern browsers and accessibility tools): Invalid response ("Math extension cannot connect to Restbase.") from server "https://en.wikipedia.org/api/rest_v1/":): {\displaystyle \overrightarrow {desync}_{direction} }

• initialize stopping profile with current values of curvature angle:

desync.pos = Failed to parse (MathML with SVG or PNG fallback (recommended for modern browsers and accessibility tools): Invalid response ("Math extension cannot connect to Restbase.") from server "https://en.wikipedia.org/api/rest_v1/":): {\displaystyle \alpha }
desync.vel = Failed to parse (MathML with SVG or PNG fallback (recommended for modern browsers and accessibility tools): Invalid response ("Math extension cannot connect to Restbase.") from server "https://en.wikipedia.org/api/rest_v1/":): {\displaystyle \dot \alpha }
desync.acc = Failed to parse (MathML with SVG or PNG fallback (recommended for modern browsers and accessibility tools): Invalid response ("Math extension cannot connect to Restbase.") from server "https://en.wikipedia.org/api/rest_v1/":): {\displaystyle \ddot \alpha }

See: Theoretical_Background

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

• during stopping:

desync.Profile();

Failed to parse (MathML with SVG or PNG fallback (recommended for modern browsers and accessibility tools): Invalid response ("Math extension cannot connect to Restbase.") from server "https://en.wikipedia.org/api/rest_v1/":): {\displaystyle \overrightarrow p = \overrightarrow {desync}_{start} \cdot cos(\alpha(t)) + \overrightarrow {desync}_{normal} \cdot sin(\alpha(t)) + \overrightarrow {desync}_{center} }

or in case of linear motion:

Failed to parse (MathML with SVG or PNG fallback (recommended for modern browsers and accessibility tools): Invalid response ("Math extension cannot connect to Restbase.") from server "https://en.wikipedia.org/api/rest_v1/":): {\displaystyle \overrightarrow p = \overrightarrow {desync}_{direction} \cdot l(t) + \overrightarrow {track}_{pos} }

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