Detecting Phase and Amplitude of a sine having known frequency - signal-processing

Question

I receive a sinusoidal data from a sensor, which is in the form (A + B(sin(n/N+a))), where N is the Total Number of Samples, plus some small noises. I know that in N samples (1000 samples), the sine wave will complete one revolution. The signal looks like this:
I want to extract the variable amplitude "B" and phase "a", using as little data as possible. In other word, I want to predict the sensor's signal as soon as possible using DSP. I have already tried "correlation" but it didn't work.
Using TMS320C000 with FPU, TMU unit.

Answer 1

First note that if your sine wave is periodic with period N, it would actually be of the form A+B*sin(2*pi*n/N + a).
For a signal with no noise and a known frequency, the unknown parameters A, B and a could be obtained with as little as 3 samples. This can be done by first solving the following linear equation system to obtain B and a:
then using back substitution to obtain A = x[0] - B*sin(a). This solution can be implemented with the following code:
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);
Since your signal includes some noise, you would get better results using a least square approximate solution to an over-determined system making use of more samples. This over-determined system can be written as:
With the following definitions:
the solution is then given as:
You then have a tradeoff between the solution's accuracy, and the number of sample used. For a solution using M samples, this can be implemented using the following C-like pseudo code:
// 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);
You may find it convenient to execute one iteration of the loop for each new sample as you receive them. Also, if you want to get continuous updates on your A, B and a estimate you simply need to execute the finalization part (the part of the code after the loop) every iteration.
Finally, for a little bit of extra robustness to input spikes, you could skip updates on b11, b12, b21, b22, z1 and z2 for large dx:
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
...