State Space Observer Control

Question

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, & 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<4; i++)         
    u += K[i]*xhat[i];

// Set Process Input:
write_to_DAC(u);

// Calculate: A*xhat + B*u
for (i=0; i<4; i++) {
    Ax_Bu[i] = 0;
    for (j=0; j<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<2; i++) {
    yhat[i]   = 0;
    y_yhat[i] = 0;  
    for (j=0; j<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<4; i++) {
    L_y_yhat[i] = 0;
    for (j=0; j<2; j++) 
        L_y_yhat[i] += L[i*2+j]*y_yhat[j];    
}   

// Update State Estimate:
for (i=0; i<4; i++) 
    xhat[i] += Ts*(Ax_Bu[i] + L_y_yhat[i]);

Answer 1

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<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<N; i++)
{  
    Bu += B[i]*u;
}

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