3
$\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]);
Improve this question
$\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$ Commented 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$ Commented 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$ Commented 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$ Commented Mar 23, 2013 at 23:19

1 Answer 1

Reset to default
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$.

Improve this answer
$\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$ Commented 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$ Commented 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$ Commented 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$ Commented 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$ Commented Mar 24, 2013 at 18:55

Start asking to get answers

Find the answer to your question by asking.

Ask question

Explore related questions

See similar questions with these tags.