cordic算法圆周系统计算sin、cos、平方和开根、atan、坐标系变换

若想计算 $sin$、 $cos$、 $\sqrt{x^2+y^2}$、 $atan$、坐标系旋转等复杂的数学运算，cordic算法提供了一种逼近的思想。cordic的圆周系统可以较精准的求解出上述数学运算，它将复杂的三角函数等数学运算通过迭代的方式分解为移位和加法，最终逼近所需要的求解精度。

一、cordic圆周系统旋转模式和向量模式

关于cordic算法的背景和理论网上已经有大量的参考文献，此处仅仅简述cordic算法的工作模式：

1.1 旋转模式

旋转模式的思想就是想通过旋转的方式，初始化$x_0=K$，$y_0=0$，$z_0=\theta$，每次迭代通过$z_i$值的正负性确定旋转方向，将已知的角度值$z_0$通过不断的迭代，逐渐逼近到$z_n=0$的过程。最终$x_n=cos\theta$，$y_n=sin\theta$，$\theta \in (-\pi \sim \pi )$。

1.2 向量模式

向量模式的思想就是初始化$x_0=A$，$y_0=B$，$z_0=0$，每次迭代通过$y_i$值的正负性确定旋转方向，最终使初始向量旋转至 $x$轴的正半轴，使$y_n$趋向于 $0$。最终$x_n=\sqrt{A^2+B^2}$，$y_n=arctan(\frac{B}{A})$。

二者模式的对比如下：

二、一些需要考虑的事项

2.1角度范围

旋转模式的目标旋转角度${\theta }_i$的取值范围是$[-99.8829°\sim 99.8829°]$ ，而计算sin、cos大部分需要的角度要大于此角度，因此可通过正余弦定理将第一二三四象限的角度全部统一到第一象限，即0-90度。即可满足$[-180°\sim 180°]$ 的实际项目需要。计算arctan的角度需要的范围完全满足，因此不需要特殊处理。

2.2输入正负

当求解$\sqrt{x^2+y^2}$时，注意输入的x，y值建议为正数，若为signed值，建议将负数转化成正数。实测发现当输入为负数时，求得的结果的误差较输入全正数时大的多。

2.3关于迭代精度

因项目要求，需要尽可能的减少资源，因此要求在保证精度的前提下通过减少迭代次数等方式进行优化来降低资源占用。
在迭代精度上，以求解$atan$的精度为例：当迭代16次时，求解$arctan1$ 的角度值如图1所示，能精确到小数点后两位。经过实测，如图2所示，当迭代8次时，通过四舍五入的方式求得的角度值基本符合项目要求（要求数值精确到整数）。

图1 求解atan迭代16次的精度

图2 求解atan迭代8次的精度

例2: 求解$\sqrt{1^2+1^2}$迭代16次的精度如图3所示, 精度非常高：

图3 求解$\sqrt{1^2+1^2}$迭代16次的精度

$\sqrt{2}$的理论值如下：

2.4坐标系旋转

由于多个雷达的点云数据需要统一到同一个坐标系下进行计算，因此用到了坐标系旋转。该算法也可利用Cordic算法思想进行求解，此时输入为$x_0$、$y_0$、$\theta$，输出取$x_n$、$y_n$。迭代方程此时变为：
$$\{\begin{array}{c}{x}_n=K(x_0+d_i{y}_0{2}^{-i})\\ y_n=K(y_0-d_i{y}_0{2}^{-i})\end{array}$$
将原有的Cordic算法稍作更改即可实现。

参考文献：

流水线求sincos：
https://blog.csdn.net/u010712012/article/details/77755567
坐标系旋转：
https://zhuanlan.zhihu.com/p/631899064
https://zhuanlan.zhihu.com/p/41263701
流水线求arctan:
https://blog.csdn.net/zwl_liang/article/details/104885263
开源ip核：
https://opencores.org/projects/cordic_atan_iq