为什么这个kalmanfilter不过滤任何东西？使用LQG控制器减少干扰和噪音

我有一个状态空间模型，如下所示：

$$
\ \ begin {bmatrix} \ dot {x} \\ \\\ dot {\ tilde {x}} \ end {bmatrix} = \开始{bmatrix} A-BL＆BL \\ 0＆A-KC \ end {bmatrix} \ begin {bmatrix} x \\ \ tilde {x} \ end {bmatrix} + \ begin {bmatrix} B＆I＆0 \\ 0＆0＆K \ end {bmatrix} \ begin {bmatrix} r \\ d \\ n \ end {bmatrix} \
$$

$ $
\ begin {bmatrix} y \\ e \ end {bmatrix} = \ begin {bmatrix} C＆0 \\ 0＆C \ end {bmatrix} \ begin {bmatrix} x \\ \ tilde {x} \ end {bmatrix} + \ begin {bmatrix} 0＆0＆0 \\ 0＆0＆1 \ end {bmatrix} \ begin {bmatrix} r \\ d \\ n \ end {bmatrix}
$$

此状态空间模型表示以下图片：


如果您仍然不了解该怎么做。观看此视频：
https://youtu.be/H4_hFazBGxU?t=2m13s

请注意估计错误：$ \ tilde {x} = x-\ hat {x} $

因此，如果您已经安装了MATLAB控制包，那么我制作了与MATLAB代码非常相似的som Octave代码。八度有一个免费的控制程序包和符号程序包。

clc
clear

% ladda bibliotek - Load GNU octave library - Free to download 
pkg load symbolic
pkg load control


% Parametrar 
m1 = 10; m2 = 7; M = 1000;
Ap = 40; Am = 20; 
Pp = 20; Pm = 10;
b1 = 3000; b2 = 1000;
L = 0.1; g = 9.82; mu = 0.3;

% Tillstånd vid statiskt - When this model are a statical point 
% the state are:
x1 = 0.65;
x2 = 0; 
x3 = 0.2;
x4 = 0;
x5 = 5*pi/180;
x6 = 0;

% Symboliska variabler
syms k1 k2 k3

% Statisk beräkning - Statistical calculations using symbolic solve
Equation1 = -k1/m1*x1 + k1/m1*x3 - b1/m1*x2 + b1/m1*x4 + Ap*10/m1*Pp - Pm*Am*10/m1*x2;
Equation2 = k1/M*x1 - k1/M*x3 + b1/M*x2 - b1/M*x4 - g*mu*x4 - k2/M*x3 + k2*L/M*x5;
Equation3 = 3*k2/(m2*L)*x3 - 3*k2/m2*x5 - 3*k3/(m2*L^2)*x5 - 3*b2/(m2*L^2)*x6 + 3*g/(2*L)*x5;


[k1, k2, k3] = solve(Equation1 == 0, Equation2 == 0, Equation3 == 0, k1, k2, k3);
k1 = double(k1);
k2 = double(k2);
k3 = double(k3);

% Dynamisk beräkning - Dynamical calculations for the state space model
A = [0 1 0 0 0 0;
     -k1/m1 (-b1/m1-Pm*Am*10/m1) k1/m1 b1/m1 0 0;
     0 0 0 1 0 0;
     k1/M b1/M (-k1/M -k2/M) (-b1/M -g*mu) k2*L/M 0;
     0 0 0 0 0 1;
     0 0 3*k2/(m2*L) 0 (-3*k2/m2 -3*k3/(m2*L^2) + 3*g/(2*L)) -3*b2/(m2*L^2)];

B = [0; Ap*10/m1; 0 ; 0; 0; 0]; % Input matrix
C = [0 1 0 0 0 0]; % Output matrix
I = [0; 1; 0 ; 0; 0; 0]; % Disturbance matrix. 

% LQR
Q = diag([0 0 0 40 0 0]);
R = 0.1;
L = lqr(A, B, Q, R); % The control law - LQR gain matrix

% LQE
Vd = diag([1 1 1 1 1 1]);
Vn = 1;
K = (lqr(A',C',Vd,Vn))'; % A way to use LQR command to compute the Kalman gain matrix

% LQG
a = [(A-B*L) B*L; zeros(6,6) (A-K*C)];
b = [B I zeros(6,1); zeros(6,1) zeros(6,1) K];
c = [C zeros(1,6); zeros(1,6) C];
d = [0 0 0; 0 0 1];
sysLQG = ss(a, b, c, d);

% Simulate the LQG with disturbance and white gaussian noise
t = linspace(0, 2, 1000);
r = linspace(20, 20, 1000);
d = 70*randn(size(t));
n = 0.1*randn(size(t));
x0 = zeros(12,1);
lsim(sysLQG, [r' d' n'], t, x0)

如果我得到噪声$ 0.1 * randn（size（t））$，则会产生此结果。 $ y1 = y $和$ y2 = e $



但是可以说我一点都没有！我得到了：


这意味着$ \ tilde {x} $根本没有任何功能！出了点问题。我在$ C $矩阵中尝试了不同的值，但没有看到估计误差。

问题：$$ $$
我的模型出了什么问题？我想控制此模型，以抵御干扰和噪音。但是现在，该模型仅接受干扰和噪声。

观察者（卡尔曼滤波器）是否也需要干扰？

编辑：$$ $$
如果我使用与以前相同的选项和代码来模拟此新状态空间模型：
$$
\ \ begin {bmatrix} \ dot {x} \\ \\\ dot {\ tilde {x}} \ end {bmatrix} = \ begin {bmatrix} A-BL＆BL \\ 0＆A-KC \ end {bmatrix } \ begin {bmatrix} x \\ \ tilde {x} \ end {bmatrix} + \ begin {bmatrix} B＆B＆0 \\ 0＆B＆K \ end {bmatrix} \ begin {bmatrix} r \\ d \\ n \ end {bmatrix} \
$$

$$
\ begin {bmatrix} y \\ e \ end {bmatrix} = \ begin {bmatrix} C＆0 \\ 0＆C \ end {bmatrix} \ begin {bmatrix} x \\ \ tilde {x} \ end {bmatrix} + \ begin {bmatrix} 0＆0＆0 \\ 0＆0＆1 \ end {bmatrix} \ begin {bmatrix} r \\ d \\ n \ end {bmatrix}
$$

我明白了。我所做的就是也增加了对卡尔曼滤波器的干扰。并插入$ I $矩阵，我将其替换为$ B $矩阵。



但是问题是，如果我仅模拟状态反馈的LQR，而不模拟卡尔曼滤波器（观察者）。我知道了：


此图片中有红色和绿色。您必须放大。红色是LQG模拟，绿色是LQR模拟（不带卡尔曼滤波器的模拟）。您可以确定kalmanfilter不过滤任何内容。为什么？

这是一个简短的代码示例：

% LQG
a = [(A-B*L) B*L; zeros(6,6) (A-K*C)];
b = [B B zeros(6,1); zeros(6,1) B K];
c = [C zeros(1,6); zeros(1,6) C];
d = [0 0 0; 0 0 1];
sysLQG = ss(a, b, c, d);

% Simulate the LQG with disturbance and no white gaussian noise
t = linspace(0, 2, 1000);
r = linspace(20, 20, 1000);
d = 2*randn(size(t));
n = 0*randn(size(t));
x0 = zeros(12,1);
[yLQG, t, xLQG] = lsim(sysLQG, [r' d' n'], t, x0);

% Simulera LQR med störning 
sysLQR = ss(A-B*L, [B B], C, [0 0])
x0 = zeros(6,1);
[yLQR, t, xLQR] = lsim(sysLQR, [r' d'], t, x0);

plot(t, yLQR, 'g', t, yLQG(:,1), 'r');