前言
本篇聚焦于 CORDIC 的定点数推广和 go 语言实现。
CORDIC 算法实现
迭代公式
CORDIC 算法的一个特点是迭代公式只包含移位和加减法，因此实现往往非常高效。而这个特点又非常匹配定点数实现，因为定点数移位、加减法都可以直接在int表示上进行，实现简单而且不会损失精度。
前文中为了计算 $\cos(\theta)$ 和 $\sin(\theta)$ ，提出了利用单位向量分量是三角函数值的性质，旋转向量逼近 $\theta$ 角来计算 $\cos(\theta)$ 和 $\sin(\theta)$ 。
我们先列出前文推导出的无缩放因子迭代公式
$$ \begin{cases} x_{i+1} = x_i - y_i \cdot d_i \cdot 2^{-i} \\ y_{i+1} = y_i + x_i \cdot d_i \cdot 2^{-i} \end{cases} $$
以及缩放因子
$$ \begin{aligned} K &= \prod_{i=0}^{N-1}\cos(a_i) \\ &\approx 0.607252935 \end{aligned} $$
在迭代完成后，将结果乘上缩放因子，就得到了旋转后的向量。 其分量就是 $cos(\theta)$ 和 $sin(\theta)$ 的近似。
$$ \begin{cases} x_r = x_N \cdot K \\ y_r = y_N \cdot K \end{cases} $$
定点数推广
上述公式里，$d_i=\plusmn 1$ 只影响符号位，$\cdot 2^{-i}$ 可以被视作右移 $i$ 比特。 其余加减乘除法都在另一篇文章 定点数算数 里进行了论证。
角度极限
前文提到利用下面的级数来逼近原角度 $\theta$，但下面的级数是收敛的，存在一个极限。
$$ \sum_{i=0}^{N} \arctan(2^{-i}) $$
如果给定的角度 $\theta$ 超出了极限值，则我们无法将向量旋转到接近 $\theta$ 角，而只能停止在极限附近。
由于我们只进行有限次迭代，可以直接写 python 脚本计算出这个极限值大约是在 1.74 rad (99°) 附近。
import math
def calc(n):
s = 0
for i in range(n):
s += math.atan(2 ** (-i))
print('迭代 %d 次，结果为 %.09f rad (%.09f°)' % (n, s, s * 180 / math.pi))
calc(10)
calc(30)
calc(100)
迭代 10 次，结果为 1.741333496 rad (99.771060036°)
迭代 30 次，结果为 1.743286619 rad (99.882965728°)
迭代 100 次，结果为 1.743286620 rad (99.882965835°)
而 $cos$ 和 $sin$ 的周期都是 $2\pi$ ，所以我们还必须找出一个办法，让输入角大于 $\frac{\pi}{2}$ 的时候也能计算出结果。
我们可以利用三角函数的基本性质：
$$ \begin{aligned} \cos(x+\pi) = -\cos(x) \\ \sin(x+\pi) = -\sin(x) \end{aligned} $$
将输入归一化到 $[-\pi,\pi]$ 范围内，然后根据原角度确定符号。
归一化
基于三角函数的基本性质，我们可以归一化输入角度到 $[-2\pi,2\pi]$ ：
$$ \begin{aligned} \cos(\theta) = \cos(\theta + 2\pi) \\ \sin(\theta) = \sin(\theta + 2\pi) \end{aligned} $$
然后利用 $\cos(x+\pi)=-\cos(x),\sin(x+\pi)=-\sin(x)$ 归一化到 $[-\frac{\pi}{2}, \frac{\pi}{2}]$
$$ \begin{aligned} \cos(\theta) &= \begin{cases} -\cos(\theta - \pi) &\text{if } \theta \gt \frac{\pi}{2} \\ -\cos(\theta + \pi) &\text{if } \theta \lt -\frac{\pi}{2} \\ \cos(\theta) &\text{if } -\frac{\pi}{2} \lt \theta \lt \frac{\pi}{2} \end{cases} \\ \sin(\theta) &= \begin{cases} -\sin(\theta - \pi) &\text{if } \theta \gt \frac{\pi}{2} \\ -\sin(\theta + \pi) &\text{if } \theta \lt -\frac{\pi}{2} \\ \sin(\theta) &\text{if } -\frac{\pi}{2} \lt \theta \lt \frac{\pi}{2} \end{cases} \\ \end{aligned} $$
角度累加
为了计算当前向量旋转到了什么角度，我们还需要一个累加器 $z$ 来记录角度，并确定下一迭代旋转方向。
由于我们每次选择的旋转角度都是确定的，即 $a_i = \arctan(2^{-i})$，我们可以预先计算出查找表以避免重复计算（还有实现 $\arctan$ 函数的麻烦）。
查找表也通过 python 脚本计算得到：
import math
for i in range(16):
v = math.atan(2 ** (-i))
print("%d. %.09f, approx %d" % (i, v, v * 65536))
0. 0.785398163, Q16 approx: 51471
1. 0.463647609, Q16 approx: 30385
2. 0.244978663, Q16 approx: 16054
3. 0.124354995, Q16 approx: 8149
4. 0.062418810, Q16 approx: 4090
5. 0.031239833, Q16 approx: 2047
6. 0.015623729, Q16 approx: 1023
7. 0.007812341, Q16 approx: 511
8. 0.003906230, Q16 approx: 255
9. 0.001953123, Q16 approx: 127
10. 0.000976562, Q16 approx: 63
11. 0.000488281, Q16 approx: 31
12. 0.000244141, Q16 approx: 15
13. 0.000122070, Q16 approx: 7
14. 0.000061035, Q16 approx: 3
15. 0.000030518, Q16 approx: 1
代码实现
一个关键点在于，CORDIC 算法不能在累加器 $z$ 正好与归一化的角度相同时提前退出，因为 K 这个常数是基于 完成了所有迭代 计算出的。即使恰好第一次旋转就对齐了角度，我们依然要继续走完所有迭代。
const (
Pi Q16 = 205887 // 3.141592653
twoPi Q16 = Pi << 1 //
halfPi Q16 = Pi >> 1 //
cordicGain Q16 = 39797 // 0.607252935
)
var (
atanLookupTable = []Q16{
51471, // 0.78539816
30385, // 0.46364761
16054, // 0.24497866
8149, // 0.12435499
4090, // 0.06241881
2047, // 0.03123983
1023, // 0.01562373
511, // 0.00781234
255, // 0.00390623
127, // 0.00195312
63, // 0.00097656
31, // 0.00048828
15, // 0.00024414
7, // 0.00012207
3, // 0.00006104
1, // 0.00003052
}
)
// cordic computes the trigonometric functions of the given angle.
// returns the cosine, sine and error.
func cordic(theta Q16) (Q16, Q16, error) {
var (
d Q16 = 1 // rotate direction, +1 for reverse clockwise, -1 for clockwise
x Q16 = MustFromInt(1) // x component of vector
y Q16 = 0 // y component of vector
z Q16 = 0 // current angle in radians
angle Q16 = theta // target angle to rotate, in between 0 and pi/2
finalSignX int = 1 // cosine sign
finalSignY int = 1 // sine sign
err error
)
for angle >= twoPi {
angle, err = Sub(angle, twoPi)
if err != nil {
return 0, 0, err
}
}
for angle < -twoPi {
angle, err = Add(angle, twoPi)
if err != nil {
return 0, 0, err
}
}
if angle > halfPi {
// rotate from 2nd quadrant to 4th quadrant
angle -= Pi
// cos(x-pi)=-cos(x)
// sin(x-pi)=-sin(x)
finalSignX = -1
finalSignY = -1
} else if angle < -halfPi {
// rotate from 3rd quadrant to 1st quadrant
angle += Pi
// cos(x+pi)=-cos(x)
// sin(x+pi)=-sin(x)
finalSignX = -1
finalSignY = -1
}
if angle > 0 {
// first rotation is reverse clockwise
d = 1
} else if angle < 0 {
// first rotation is clockwise
d = -1
} else if angle == 0 {
return MustFromInt(1), MustFromInt(0), nil
}
for i := range len(atanLookupTable) {
var (
nx Q16 // new x
ny Q16 // new y
err error
)
intermediate := y >> i
nx, err = Sub(x, d*intermediate)
if err != nil {
return 0, 0, err
}
intermediate = x >> i
ny, err = Add(y, d*intermediate)
if err != nil {
return 0, 0, err
}
x = nx
y = ny
z += d * atanLookupTable[i]
if z > angle {
d = -1
} else if z < angle {
d = 1
}
}
x, err = Mul(x, cordicGain)
if err != nil {
return 0, 0, err
}
y, err = Mul(y, cordicGain)
if err != nil {
return 0, 0, err
}
return Q16(finalSignX) * x, Q16(finalSignY) * y, nil
}
func Cos(theta Q16) (Q16, error) {
cos, _, err := cordic(theta)
return cos, err
}
func Sin(theta Q16) (Q16, error) {
_, sin, err := cordic(theta)
return sin, err
}
积累误差
值得注意的是这个算法对大角度输入会有积累误差，比如原始输入是 MustFromFloat(100*math.Pi+math.Pi/4) 的时候，输入是从较为精确的 float64 得到的 Q16 值，而算法内部的归一化过程每次减去 Q16 的 $2\pi$ 都是会累积出极小的误差，使得归一化的结果与 $\frac{\pi}{4}$ 出现无法忽略的差异。
原因在于 Q16 版本的 $\pi$ 存在的固有误差被放大。所以使用中，最好不要使用大弧度的输入，以避免误差累积。