2
$\begingroup$

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]);
$\endgroup$
4
  • $\begingroup$ What is there to figure out? What can't you figure out? Does looking at some matlab code help in your understanding? $\endgroup$
    – Peter K.
    Mar 23, 2013 at 18:22
  • $\begingroup$ 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. $\endgroup$ Mar 23, 2013 at 19:20
  • $\begingroup$ 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). $\endgroup$
    – Peter K.
    Mar 23, 2013 at 22:23
  • $\begingroup$ $ 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. $\endgroup$ Mar 23, 2013 at 23:19

1 Answer 1

1
$\begingroup$

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

$\endgroup$
9
  • $\begingroup$ Thanks for the explanation Peter. $C$ is 2x4. And does the order of the loop in my original post look correct? $\endgroup$ Mar 24, 2013 at 0:12
  • $\begingroup$ 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. $\endgroup$
    – Peter K.
    Mar 24, 2013 at 0:18
  • $\begingroup$ 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)) $\endgroup$ Mar 24, 2013 at 0:49
  • $\begingroup$ That Ts term often shows up. You'd have to point me to the origin of the equations before I could say unequivocally. $\endgroup$
    – Peter K.
    Mar 24, 2013 at 18:24
  • $\begingroup$ I got the algorithm from pages 136-137 on this document: cds.caltech.edu/~murray/courses/cds101/fa04/caltech/… $\endgroup$ Mar 24, 2013 at 18:55

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge that you have read and understand our privacy policy and code of conduct.

Not the answer you're looking for? Browse other questions tagged or ask your own question.