State Space Observer Control I am attempting to implement observer based state feedback in C and can't figure it out.

Here is pseudocode of the algorithm:

//Initialize Stuff

loop:

//read process variable                                                                                 
y = read(ch1)  

//compute control variable                                                                      
u = -K * xhat 

//set process input                                                                             
write(u) 

//update state estimate                                                                                  
xhat = xhat + Ts * (A*xhat + B*u + L*(y-C*xhat))

// repeat loop



xhat is the estimated state                                                                            
K is the feedback gain                                               
L is the observer gain                                               
y is the system output                                               
u is the system input                                                
Ts is the sampling period                                            
A, B, &amp; C are the state space matrices


Is this how observer feedback is supposed to be implemented?

Here is the actual code I have for the loop:

// Get Process Output from Sensors:
y[0] = sensor1();
y[1] = sensor2();

// Compute Control Variable:
u = 0;  
for (i=0; i&lt;4; i++)         
    u += K[i]*xhat[i];

// Set Process Input:
write_to_DAC(u);

// Calculate: A*xhat + B*u
for (i=0; i&lt;4; i++) {
    Ax_Bu[i] = 0;
    for (j=0; j&lt;4; j++)
        Ax_Bu[i] += A[i*4+j]*xhat[j];    
    Ax_Bu[i] += B[i]*u; 
}

// Calculate: y-y_hat
for (i=0; i&lt;2; i++) {
    yhat[i]   = 0;
    y_yhat[i] = 0;  
    for (j=0; j&lt;4; j++) {
        yhat[i] +=  C[i*4+j]*xhat[j]; // Calculate yhat = C*xhat
        y_yhat[i] = y[i]-yhat[i];     // Calculate the 'Innovation'
    }             
}

// Calculate: L*(y-y_hat)
for (i=0; i&lt;4; i++) {
    L_y_yhat[i] = 0;
    for (j=0; j&lt;2; j++) 
        L_y_yhat[i] += L[i*2+j]*y_yhat[j];    
}   

// Update State Estimate:
for (i=0; i&lt;4; i++) 
    xhat[i] += Ts*(Ax_Bu[i] + L_y_yhat[i]);
Assuming $y$ and $u$ are scalars, then the first equation:

$$
u = -K \hat{x}
$$

is just a vector-vector multiplication and can be implemented in C as:

u = 0.0;
for (i=0; i&lt;N; i++)
{  
    u += xhat[i]*K[i];
}


assuming that $K$ is $1 \times N$ and that $\hat{x}$ is $N \times 1$.

The multiplication C*xhat can be implemented similarly (because I suspect that $C$ is really $1 \times N$ rather than $2 \times N$).

The multiplications L*(y-C*xhat) and B*u are vector-scalar multiplications which, for the latter, would look like:

Bu = 0.0;
for (i=0; i&lt;N; i++)
{  
    Bu += B[i]*u;
}


The multiplication  A*xhat  will be slightly different because $A$ is $N \times N$.