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:

//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]);

• What is there to figure out? What can't you figure out? Does looking at some matlab code help in your understanding? – Peter K. Mar 23 '13 at 18:22
• I am confused in the implementation. With the pseudo code I wrote the system input u seems to just keep increasing in every iteration of the loop. So my question is if what I have written is a correct implementation. – Cosmo Kramer Mar 23 '13 at 19:20
• Well, since you haven't shown the equations for $K$ and $L$, you can't really say u seems to just keep increasing. It's a feedback system: and $K$, $L$, $xhat$ are vectors, not scalars (unless it's a very simple system). – Peter K. Mar 23 '13 at 22:23
• $u$ is a scalar, and all others are vectors/matrices: $K$ is 4x1, $L$ is 4x2, $A$ is 4x4, $B$ is 4x1, $C$ is 2x4. – Cosmo Kramer Mar 23 '13 at 23:19

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$.

• Thanks for the explanation Peter. $C$ is 2x4. And does the order of the loop in my original post look correct? – Cosmo Kramer Mar 24 '13 at 0:12
• OK, that means $y$ must be $2 \times 1$; I assumed it was scalar. No real matter. Regarding the loop order: I would have thought the LAST thing to do would be write(u)... but it depends on the indices in the original equations. – Peter K. Mar 24 '13 at 0:18
• In the line where the state estimate is updated is it proper to multiply that term by the sampling period? I am referring to: Ts * (Axhat + Bu + L*(y-C*xhat)) – Cosmo Kramer Mar 24 '13 at 0:49
• That Ts term often shows up. You'd have to point me to the origin of the equations before I could say unequivocally. – Peter K. Mar 24 '13 at 18:24
• I got the algorithm from pages 136-137 on this document: cds.caltech.edu/~murray/courses/cds101/fa04/caltech/… – Cosmo Kramer Mar 24 '13 at 18:55