信息处理 - 状态空间观察者控制 - 吾爱随笔录

状态空间观察者控制

信息处理控制系统

2022-02-07 22:50:18

我正在尝试在 C 中实现基于观察者的状态反馈，但无法弄清楚。

下面是算法的伪代码：

//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 是估计的状态
K 是反馈增益
L 是观察者增益
y 是系统输出
u 是系统输入
Ts 是采样周期
A、B 和 C 是状态空间矩阵

这是应该如何实施观察者反馈的吗？

这是我为循环所拥有的实际代码：

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

1个回答

假设 $y$ 和 $u$ 是标量，那么第一个方程：

u = - K \hat{x}

$u = -K \hat{x}$

只是一个向量向量乘法，可以实现C为：

u = 0.0;
for (i=0; i<N; i++)
{  
    u += xhat[i]*K[i];
}

假如说 $K$ 是 $1 \times N$ 然后 $\hat{x}$ 是 $N \times 1$ .

乘法C*xhat可以类似地实现（因为我怀疑 $C$ 是真的 $1 \times N$ 而不是 $2 \times N$ ）。

乘法L*(y-C*xhat)和B*u是向量标量乘法，对于后者，看起来像：

Bu = 0.0;
for (i=0; i<N; i++)
{  
    Bu += B[i]*u;
}

乘法 A*xhat 会略有不同，因为 $A$ 是 $N \times N$ .

其它你可能感兴趣的问题

上一篇使用 trainCascadeObjectDetector 训练自定义分类器下一篇用于图像压缩的 Contourlet 变换