我从传感器接收到一个正弦波形式的数据,其形式为(A + B(sin(n/N+a))),其中N是样本总数,加上一些小噪声。我知道在N个样本(1000个样本)中,正弦波将完成一次旋转。信号看起来像这样:
使用带有FPU、TMU单元的TMS320C000。
使用带有FPU、TMU单元的TMS320C000。
N
,实际上其形式应为A+B*sin (2*pi*n/N+a)
。A
、B
和a
可以通过仅有3个样本来获取。这可以通过首先解决以下线性方程组来获得B
和a
来完成:
然后使用回代法得出 A = x[0] - B*sin(a)
。可以使用以下代码实现该解决方案:double K = 2*PI/N;
// setup matrix
// [sin(1/N)-sin(0/N) cos(1/N)-cos(0/N)]
// [sin(2/N)-sin(1/N) cos(2/N)-cos(1/N)]
double a11 = sin(K);
double a12 = cos(K)-1;
double a21 = sin(2*K)-sin(K);
double a22 = cos(2*K)-cos(K);
// Compute 2x2 matrix inverse, and multiply by delta x vector
// see https://www.wolframalpha.com/input/?i=inverse+%7B%7Ba,+b%7D,+%7Bc,+d%7D%7D
double d = 1.0/(a11*a22-a12*a21);
double y0 = d*(a22*(x[1]-x[0])-a12*(x[2]-x[1])); // B*cos(a)
double y1 = d*(a11*(x[2]-x[1])-a21*(x[1]-x[0])); // B*sin(a)
// Solve for parameters
double B = sqrt(y0*y0+y1*y1);
double a = atan2(y1, y0);
double A = x[0] - B*sin(a);
由于你的信号中包含某些噪声,因此使用最小二乘逼近超定系统的解利用更多样本可以获得更好的结果。这个超定系统可以写成:
使用以下定义: 解决方案如下:您在解决方案的准确性和样本数量之间需要进行权衡。对于使用M
个样本的解决方案,可以使用以下类似C语言的伪代码实现:
// Setup initial conditions
double K = 2*PI/N;
double s = sin(K);
double c = cos(K);
double sp = s;
double cp = c;
double sn = s*cp + c*sp;
double cn = c*cp - s*sp;
double t1 = s;
double t2 = c-1;
double b11 = 0.0;
double b12 = 0.0;
double b21 = 0.0;
double b22 = 0.0;
double z1 = 0.0;
double z2 = 0.0;
double dx = 0.0;
// Iterative portion
for (int i = 0; i < M-1; i++)
{
// B_{i,j} = (A^T A)_{i,j} = sum_{k} A_{k,i} A_{k,j}
// For a 2x2 matrix B and a given "k",
// we have two values t1 & t2 which represent A_{k,1} and A_{k,2}
b11 += t1*t1;
b12 += t1*t2;
b21 += t2*t1;
b22 += t2*t2;
// Update z_i = (A^T \Delta x)_i = \sum_k A_{k,i} (\Delta x)_i
dx = x[i+1]-x[i];
z1 += t1 * dx;
z2 += t2 * dx;
// Update t1 & t2 for next term
t1 = sn-sp;
t2 = cn-cp;
// Iteratively compute sin(2*pi*k/N) & cos(2*pi*k/N) using trig identities:
// sin(x+2pi/N) = sin(2pi/N)*cos(x) + cos(2pi/N)*sin(x)
// cos(x+2pi/N) = cos(2pi/N)*cos(x) - sin(2pi/N)*sin(x)
sp = sn;
cp = cn;
sn = s*cp + c*sp;
cn = c*cp - s*sp;
}
// Finalization
// Compute inverse of B
double dinv = 1.0/(b11*b22-b12*b21);
double binv11 = b22*dinv;
double binv12 = -b21*dinv;
double binv21 = -b12*dinv;
double binv22 = b11*dinv;
// Compute inv(B)*A^T \Delta x which gives us the 2D vector [B*cos(a) B*sin(a)]
double y1 = binv11*z1 + binv12*z2; // B*cos(a)
double y2 = binv21*z1 + binv22*z2; // B*sin(a)
// Solve for "B", "a" and "A" parameters
double B = sqrt(y1*y1+y2*y2);
double a = atan2(y2, y1);
double A = x[0] - B*sin(a);
A
、B
和a
估计值,只需在每次迭代后执行最终化部分(即循环后的代码部分)即可。dx
较大时跳过对b11
、b12
、b21
、b22
、z1
和z2
的更新。dx = x[i+1]-x[i];
// either use fixed threshold (requires manual tuning) for simplicity
// or update threshold dynamically as a fraction of B once a reasonable estimate of
// B is obtained.
if (abs(dx) < threshold)
{
b11 += t1*t1;
b12 += t1*t2;
b21 += t2*t1;
b22 += t2*t2;
z1 += t1 * dx;
z2 += t2 * dx;
}
// always update t1, t2, sp, cp, sn, cn
...