Arctan(x) using CORDIC

1. The problem and some solutions

Sometimes, especially when navigating a robot, you'll have to determine a certain angle from the tangens-value. This is challenging because the RCX only works with small integer variables within the limits of -32768 and 32767. The arctan-function has to cover the range of 0...45 degrees e.a values of x between 0 and 1, first to avoid overflow errors or a division by zero problem at angles around 90 degrees. If the arctan has to be found for an angle alpha between 45 and 90 degrees (e.a. values of x above 1), you'll search for the arctan(1/x) and extract alpha=90-arctan(1/x).

There are several possibilities to do the job, such as a look-up table, a linear or polynomial approximation or even a CORDIC algorithm. As you can see in the picture above, linear approximation will produce some big errors (~5°). Cumulated with other error sources, this will probably spoil your result.

An acceptable result is given by polynomial approximation. The least squares technique gives the formula:

y = -0.30097 + 0.61955*x - 0.001659*x*x

x :number from 0..100%

y : angle from 0..45° as approximation of the atan function.

This formula has to be simplified for the RCX-use to always stay in the small-integer limits:

y ={ [-150 + 310 * x - (x*x DIV 2) - (x*x DIV 3) ] DIV 50 + 5 } DIV 10

{Here the RCX-Code}

#define(x,1)
#define(x^2,2)
#define(result,3)
#define(time,4)

setvar(x^2,var,x)
mulvar(x^2,var,x)
setvar(result,var,x^2)
divvar(result,con,-3)
divvar(x^2,con,2)
subvar(result,var,x^2)
mulvar(x,con,310)
sumvar(result,var,x)
subvar(result,con,150)
divvar(result,con,50)
sumvar(result,con,5)
divvar(result,con,10)

This polynomial approximation gathers all the advantages: high speed (0.1 seconds), low memory, good accuracy!

We also tried a tricky approximation of This subroutine has been incorporated in both the second compass-sensor design and the pathfinder.

Here  the integral algorithm programmed first in PASCAL, then in RCX-Code.

{dx of the integral is set equal to 1} 
{the tangens must be entered in x as a percent-value e.a. a value between 0 and 100} 

FUNCTION arc_tan(x : smallint) : smallint;
VAR i : smallint;
BEGIN
result:=0; 
IF x>=0 THEN 
  BEGIN 
   FOR i:=1 TO x DO 
      BEGIN 
          result:=result+10000/(100+i*i/100);  
      END; 
    result:=result/78*45/100;  {To convert rad --> deg, first do the division to stay within the small integer limits.} 
  END;
END;
{small integer variables: x, y ,f_y ,i, result } 

{the tangens must be entered in x as a percent-value e.a. a value between 0 and 100} 

beginofsub(arctan) 
   setvar(result,con,0)  {clear} 
   setvar(i,con,0)            {arctan as integral of f(y)=1/(1+y*y)} 
   while(var,i,LT,var,x)    {while i<x} 
      sumvar(i,con,1) 
      setvar(y,var,i) 
      mulvar(y,var,y)     {square, now we have 4 to 5 digits} 

      divvar(y,con,10)   {div 100, rounded} 
      sumvar(y,con,5) 
      divvar(y,con,10) 

      sumvar(y,con,100)   {we take f(y)=10000/(100+y*y/100)} 
      setvar(f_y,con,10000) 
      divvar(f_y,var,y) 
      sumvar(result,var,f_y) 
   endwhile() 
  
    divvar(result,con,78) {conversion: rad --> degrees} 
    mulvar(result,con,45) 
    divvar(result,con,10) {div 100, rounded} 
    sumvar(result,con,5) 
    divvar(result,con,10) 
endofsub() 

{the angle is to be read out of result} 

 
This is quite an interesting program-design. It does not affect much memory and has a precision of 1°, which is sufficient with the PEWATRON-sensor. But how about speed? In fact there are two major inconveniences that appear: first, the computing time is proportional to the input-value x, second, the RCX beeing a rather slow computer, it needs 3,6 seconds to calculate the arctan(1), which is quite a lot. In the case of the PEWATRON compass-programming this doesn't matter, because the sensor's recover-time stays around 2 seconds after turning, but in the case of the pathfinder this makes the robot react with an unstable delay. The actual course is never the real instant course but the course of some tenth of a second or even more in the past.

This delay-problem can be surrounded by a look-up table, where only few instructions have to be done.

Here an example of a look-up table. This program is not elegant, but it is very quick: 45° need only 0.2 seconds.
 

{x is a value from 0..100%}
BeginOfSub(Arctan) 
  IF(var,x,LT,con,2) 
      setvar(result,con,0) 
  ELSE() 
      IF(var,x,LT,con,4) 
          setvar(result,con,1) 
      ELSE() 
            IF(var,x,LT,con,6) 
                setvar(result,con,2) 
            ELSE() 
                      IF(var,x,LT,con,8) 
                          setvar(result,con,3) 
                      ELSE() 
                            etc..etc..etc... 
                      ENDIF()  
            ENDIF() 
      ENDIF() 
  ENDIF() 
EndOfSub()

Download the RCX-Code

2. The CORDIC solution

CORDIC (COordinate Rotation DIgital Computer) is an iterative algorithm for calculating trigonometric functions and has been developed by J.E. Volder in 1959 (see "CORDIC Trigonometric Computing Technique", IRE Transactions on Electronic Computers, EC-8, Sept. 1959). It calculates the trig and even hyperbolic functions to any desired precision. The central idea is to rotate the phase of a complex number by a succession of constant values. Yes you should have paid attention in maths!
 

Given a complex value Z with the real component a, the imaginary component b, the phase phi:

Z = a + bj tan(phi) = b / a, a<>0 if a=0 then (if b>0 phi = 90° else phi = - 90°)
magnitude(Z) = SQRT(a^2 + b^2) cos(phi) = a / m(Z) sin(phi) = b / m(Z)

Given a second complex value W = c + dj

Z . W = (a.c - b.d) + (a.d + b.c) j

Remember that when you multiply a pair of complex numbers, their phases are added and their magnitudes multiply. Similary, when you multiply a complex number by the conjugate of the other, the phase of the conjugated one is substracted, but the magnitudes still multiply. The conjugate of Z, Z* = a - bj.
 

To rotate by +90°, multiply by R = j -->Z' = -b + aj. To rotate by -90°, multiply by R = -j --> Z' = b - aj
To rotate by an angle less than 90°, the CORDIC idea asks to multiply by numbers of the form R = 1 + kj, where k will be decreasing powers of 2, starting with k = +/-2^0 = +/- 1.0 So k will successively be equal to +/-1.0, 0.5, 0.25, 0.125, etc. Generally k = +/- 2^-L, where L designates the power of two: 0, 1, 2, 3 ... The phase of R = atan(k). Rotating will be equivalent to multiplying Z' = Z.R = (a+bj).(1+kj) = (a-b.k) + (a.k+b)j.

For better comprehension have a look at the table below. You'll see that every rotation causes the magnitude to grow. The factor is dependent on L. If successive rotations are completed with L increasing progressively, the relative gain to the initial magnitude (called the CORDIC gain) grows, but tends to a limit value of 1.6467...
 

L k=2^L R=1 + kj Phase of R in ° m(R) CORDIC Gain
0 1.0 1+1.0j 45.00000 1.41421356 1.41421356
1 0.5 1+0.5j 26.56505 1.11803399 1.58113883
2 0.25 1+0.25j 14.03624 1.03077641 1.62980060
3 0.125 1+0.125j 7.12502 1.00778222 1.64244841
4 0.0625 1+0.0625j 3.57633 1.00195122 1.64568892
5 0.03125 1+0.03125j 1.78991 1.00048816 1.64649228
6 0.015625 1+0.015625j 0.89517 1.00012206 1.64669325
7 0.007813 1+0.007813j 0.44761 1.00003052 1.64674351
... ... ... ... ... ...

Exercise 1

We know the complex number Z = a + bj, for example Z0 = 2 + 7j. Determine the phase phi.

CORDIC Solution:

This exercise is equivalent to fixing the arctan(2/7).

1. The CORDIC algorithm first lets you rotate by +90° if the imaginary argument b<0 and by -90° if the b>0. Example: Z1 = 7 - 2j

2. Then rotate successively by atan(k), where k=+/-2^-L, L=0, 1, 2, 3, 4,.... . The decision of sign is always taken on the sign of the imaginary argument b. Obviously phii will tend towards 0. And this is the trick: we only have to add the well known values (which we poll from a look-up table) of the atan(ki). At every computation these numbers will have the same absolute values. What only will change from case to case is the actual sign of ki. If the actual phase of the rotated vector has sensibly reached 0, the opposite of sum=+/-90+S(atan(ki)) gives us the phase of the initial complex number! ATTENTION: for this whole operation the real argument a must always be positive, e. a. angle values from -90 to +90°!!!

For better understanding have a look at the table below, or better try to calculate it by your own:
 

i L real arg. ai  imag. arg.bi  bi>0? --> sign ki  atan(ki) in ° "+/- 90 + S(atan(ki)) 
1   2 7 -1     -90
2 0 7 -2 1 1 45 -45
3 1 9 5 -1 -0,5 -26,5650512 -71,56505118
4 2 11,5 0,5 -1 -0,25 -14,0362435 -85,60129465
5 3 11,625 -2,375 1 0,125 7,12501635 -78,4762783
6 4 11,921875 -0,921875 1 0,0625 3,57633437 -74,89994392
7 5 11,97949219 -0,17675781 1 0,03125 1,78991061 -73,11003331
8 6 11,98501587 0,19760132 -1 -0,015625 -0,89517371 -74,00520702
9 7 11,98810339 0,01033545 -1 -0,0078125 -0,44761417 -74,45282119
10 8 11,98818414 -0,08332161 1 0,00390625 0,2238105 -74,22901069
11 9 11,98850961 -0,03649277 1 0,00195313 0,11190568 -74,11710502
12 10 11,98858089 -0,01307771 1 0,00097656 0,05595289 -74,06115212
13 11 11,98859366 -0,00137011 1 0,00048828 0,02797645 -74,03317567
14 12 11,98859433 0,00448369 -1 -0,00024414 -0,01398823 -74,0471639
15 13 11,98859542 0,00155679 -1 -0,00012207 -0,00699411 -74,05415801
               
    verific. tan(j) = bi/ai  atan(j)       
      3,5 74,0546041      

Download the Excel sheet

3. This computation gives us also the magnitude through the formula:

m(Z) = au / Cgain(u), where u is the index of the last operation with phiu=0(+/-tolerance), bu= 0 and au = m(Zu)

This formula may be reduced to:

m(Z) = au / 1.6467, as shown above.

An RCX-program for this algorithm looks like:
 
 

{this is a CORDIC iteration for
the arctan function}
 
{...local variables F*X..}
#define(p,20)
#define(q,21)
#define(r,22)
#define(h,23)
{...argument variables...}
#define(F,10)
#define(X,11)
#define(res,12)
{...main variables...}
#define(value1,1)
#define(value2,2)
#define(s,3)
#define(k,4)
#define(phase,5)
#define(sum_phase,6)
#define(temp,7)
#define(L,8)
 
#define(time,9) {for test use}
{...subroutines}
#define(F*X,1)
 
beginoftask(main)
cleartimer(timer_1)
setvar(value1,con,260) {test values, may be middle-adjusted raw-values of compass-sensor}
setvar(value2,con,50)
 
{+/- 90° rotation}
sgnvar(s,var,value2) {sign decision}
mulvar(s,con,-1) {has to be the opposite}
 
setvar(sum_phase,con,900) {reset}
mulvar(sum_phase,var,s)
 
setvar(temp,var,value1)
setvar(value1,var,s)
mulvar(value1,con,-1)
mulvar(value1,var,value2)
 
setvar(value2,var,temp)
mulvar(value2,var,s)
{iteration }
setvar(L,con,0)
while(var,L,LT,con,9)
sumvar(L,con,1) {index L}
 
if(var,L,eq,con,1) {look-up table}
setvar(k,con,10000)
setvar(phase,con,450) 
endif()
 
if(var,L,eq,con,2) {look-up table}
setvar(k,con,5000)
setvar(phase,con,266) 
endif()
 
if(var,L,eq,con,3) {look-up table}
setvar(k,con,2500)
setvar(phase,con,140) 
endif()
 
if(var,L,eq,con,4) {look-up table}
setvar(k,con,1250)
setvar(phase,con,71) 
endif()
 
 
if(var,L,eq,con,5) {look-up table}
setvar(k,con,625)
setvar(phase,con,36) 
endif()
 
if(var,L,eq,con,6) {look-up table}
setvar(k,con,313)
setvar(phase,con,18) 
endif()
 
if(var,L,eq,con,7) {look-up table}
setvar(k,con,156)
setvar(phase,con,9) 
endif()
 
if(var,L,eq,con,8) {look-up table}
setvar(k,con,78)
setvar(phase,con,4) 
endif()
 
sgnvar(s,var,value2) {sign decision}
mulvar(s,con,-1) {opposite sign}
mulvar(k,var,s)
mulvar(phase,var,s)
 
sumvar(sum_phase,var,phase) {here ten times the opposite of the arctan value will be found}
 
setvar(temp,var,value1)
setvar(F,var,k)
setvar(x,var,value2)
gosub(F*X)
subvar(value1,var,res)
setvar(x,var,temp)
gosub(F*X)
sumvar(value2,var,res)
endwhile()
setvar(time,timer,timer_1)
endoftask()
 
 
 
 
 
{============================================}
{ this subroutine multiplies an integer value x
from -1638 to +1638 with a floating variable F (without point)
of the form p. ex. 10448 which means 1.0448
The range of F must be: -19999 19999}
 
beginofsub(F*X)
 
{.....le'ts take x=909 as an example.....}
setvar(q,var,F) {10448}
divvar(q,con,1000) {10448 div 1000=10}
setvar(res,var,q) {10}
mulvar(res,var,x) {10*909=9090}
mulvar(q,con,-1000) {10*(-1000)=-10000}
sumvar(q,var,F) {-10000+10448=448}
 
setvar(r,var,q) {448}
divvar(q,con,100) {448 div 100=4}
setvar(h,var,q) {4}
mulvar(h,var,x) {4*909=3636}
mulvar(q,con,-100) {4*100=-400}
sumvar(r,var,q) {448-400=48}
 
setvar(q,var,r) {48}
divvar(q,con,10) {48 div 10=4}
setvar(p,var,q) {4}
mulvar(p,var,x) {4*909=3636}
divvar(p,con,10) {3636 div 10=363}
sumvar(h,var,p) {3636+363=3999}
mulvar(q,con,-10) {4*(-10)=-40}
sumvar(r,var,q) {48-40=8}
mulvar(r,var,x) {8*909=7272}
divvar(r,con,100) {7272 div 100=72}
sumvar(h,var,r) {3999+72=4071}
divvar(h,con,10) {4071 div 10=407}
sumvar(res,var,h) {9090+407=9497}
sumvar(res,con,5) {9497+5=9503}
divvar(res,con,10) {9503 div 10=950} 
endofsub()

 
Using this solution, we have a rather disappointing result: duration of a computation: 2.2 seconds; accuracy: 1-2°; high memory use. But we have learned some exciting program-technique!

Download the RCX-Code.

Exercise 2 : Calculate the square-root of a number x using CORDIC.

CONCLUSION

The best solution are the look-up table and the polynomial approximation.

 RetourMain page