Calculating velocity

Join Date
Apr 2002
Location
No income tax, no capital gains tax. Freedom!
Posts
8,351
I worked out a simple filter for calculating velocity. The advantage is that one gets some filtering with little or no phase delay. This method isn't perfect just better than some other options. Low pass filters are great but they introduce phase lag that is unacceptable in dynamic situations. I have mentioned this technique before where one does a least squares fit for a second order polynomial s:=a+b*t+c*t^2 to the last 10 encoder readings. I chose 10 is arbitrary but most people liked 10. I can then take the derivative of the position function to get v:=b+2*c*t. The current encoder reading is always time 0 older readings are and negative times but we only care about the current time. If t is set to 0 then v=b. Simple. This means that only the coefficient b needs to be calaculated and it is just a weight average of the last 10 readings. I wrote this in SCL but one can probably implement this in a Rockwell AOI too or even a compute block.

Code:
(* CALCULATE VELOCITY *)
(* 090606 *)
// The current velocity is calculated by using an weighted average of
// the last 10 readings.  The coefficients were derived by fitting a
// second order equation to the last 10 points and the taking the derivative
// at the last or current position. This method provide a filtered velocity
// with little or no lag or phased delay due to filtering.
// This function was tested using a sine function to generate position that were
// used as NewPos parameters.  The resulting velocity should be close to the
// velocity calculated by taking the derivative of sine function. This
// filter isn't perfect, just better than using an low pass filter when phase
// delays are critical.
FUNCTION_BLOCK fbCalcVel
TITLE='fbCalcVel'
VAR_INPUT
    NewPos: REAL;                       // the current position
    DeltaT: REAL;                       // assumed to be fixed      
END_VAR
VAR
    arPos: ARRAY[0..9] OF REAL;
    OldDeltaT: REAL:=0;
    InvDeltaT: REAL;
END_VAR
VAR_TEMP
    I: INT;
END_VAR
VAR_OUTPUT
    Velocity: REAL;
END_VAR
    IF OldDeltaT<>DeltaT THEN           // check for change
        OldDeltaT:=DeltaT;
        InvDeltaT:=1.0/DeltaT;          // for speed, could use updates per second instead
        FOR I:=0 TO 9 BY 1 DO
            arPos[I]:=NewPos;           // initialize position history, assumed stopped
        END_FOR;
    END_IF;
    arPos[9]:=arPos[8];                 // crude but faster than circular queue
    arPos[8]:=arPos[7];
    arPos[7]:=arPos[6];
    arPos[6]:=arPos[5];
    arPos[5]:=arPos[4];
    arPos[4]:=arPos[3];
    arPos[3]:=arPos[2];
    arPos[2]:=arPos[1];
    arPos[1]:=arPos[0];
    arPos[0]:=NewPos;
    Velocity:=(0.25909090909090909091*arPos[0]  
              +0.11060606060606060606*arPos[1]
              -0.37878787878787878788e-2*arPos[2]
              -0.84090909090909090909e-1*arPos[3]
              -0.13030303030303030303*arPos[4]
              -0.14242424242424242424*arPos[5]
              -0.12045454545454545455*arPos[6]
              -0.64393939393939393939e-1*arPos[7]
              +0.25757575757575757576e-1*arPos[8]
              +0.15000000000000000000*arPos[9]
              )*InvDeltaT;              
END_FUNCTION_BLOCK
 
Quick question

I realize this is an old thread, but I'm attempting to use this block and have a small question.

I have placed this block in OB35 which I've configured to execute every 100ms.

I'm curious about the usage of the input DeltaT. Is this the sample time in which the block is executed in seconds?
 
I realize this is an old thread, but I'm attempting to use this block and have a small question.

I have placed this block in OB35 which I've configured to execute every 100ms.

I'm curious about the usage of the input DeltaT. Is this the sample time in which the block is executed in seconds?
If your sample time is in seconds then the velocity will have units of distance/second.

DeltaT should be 0.1 seconds in your example. If your position are in mm then the units will be mm/sec.

Hopefully the speed you are trying to estimate will not change much during the filter time. If so then you may want to execute the filter more often like every 10 milliseconds.
 
I think don't think the cylinder will move much in 100ms. I can always change the sample time of the OB though and adjust DeltaT accordingly.

Thanks a bunch!!
 
Last edited:

Similar Topics

Seems like this should be a simple thing but I’m struggling with it. The basis is that I am trying to detect if a linear assembly is stopped or...
Replies
6
Views
3,048
I'm working on calculating the velocity of a cylinder. The cylinder can extend and retract. I've got a solution that I *think* works, but I'm...
Replies
3
Views
2,103
This application has a motor with encoder feedback that drives a linear actuator that moves in/out, and is at roughly 45 degs from horiz. As the...
Replies
19
Views
1,296
I need to keep a running pass/fail yield of the previous 5,000 parts produced. I have used this formula before to calculate average: AvgValue =...
Replies
6
Views
2,113
Does anyone know how to calculate the savings from now needing to run an air compressor all day? Basically I have a design that replaced 6 * 1"...
Replies
26
Views
4,749
Back
Top Bottom