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