卡尔曼滤波状态观测器设计Matlab程序

% 定义系统的状态空间模型
A = [0.8, 0.2; -0.2, 0.5];
B = [1; 0.5];
C = [1, 0];
D = 0;
sys = ss(A, B, C, D);

% 定义观测噪声协方差和状态噪声协方差
RN = 0.1;  % 观测噪声协方差
QN = 0.01; % 状态噪声协方差

% 设计卡尔曼滤波器
[K, P, E] = kalman(sys, QN, RN); 

% 生成含有未知扰动的状态观测数据
T = 100; % 时间步数
x0 = [0; 0]; % 初始状态
u = randn(1, T); % 随机控制输入
w = sqrt(QN) * randn(2, T); % 状态噪声
v = sqrt(RN) * randn(1, T); % 观测噪声
x = zeros(2, T); % 真实状态
y = zeros(1, T); % 观测值
for k = 1:T
    x(:,k+1) = A * x(:,k) + B * u(k) + w(:,k);
    y(:,k) = C * x(:,k) + D * u(k) + v(:,k);
end

% 初始化状态估计
x_est = zeros(2, T+1); % 估计状态
P_est = zeros(2, 2, T+1); % 估计状态协方差
x_est(:,1) = x0;
P_est(:,1) = P;

% 使用卡尔曼滤波器进行状态估计
for k = 1:T
    % 预测步骤
    x_pred = A * x_est(:,k) + B * u(k);
    P_pred = A * P_est(:,:,k) * A' + QN;
    
    % 更新步骤
    Kk = P_pred * C' / (C * P_pred * C' + RN);
    x_est(:,k+1) = x_pred + Kk * (y(:,k) - C * x_pred);
    P_est(:,:,k+1) = (eye(2) - Kk * C) * P_pred;
end

% 绘制结果
t = 0:T;
figure;
plot(t, x(1,:), 'b-', t, x_est(1,:), 'r--');
xlabel('时间');
ylabel('状态1');
legend('真实状态', '估计状态');
title('状态1估计结果');

figure;
plot(t, x(2,:), 'b-', t, x_est(2,:), 'r--');
xlabel('时间');
ylabel('状态2');
legend('真实状态', '估计状态');
title('状态2估计结果');
figure;
plot(t(1:T), y, 'b-', t, x_est(1,:), 'r--');
xlabel('时间');
ylabel('观测值与状态1估计');
legend('观测值', '状态1估计');
title('观测值与状态1估计结果');

% 计算估计误差
est_error = x - x_est;

% 绘制估计误差
figure;
plot(t, est_error(1,:), 'b-', t, est_error(2,:), 'r--');
xlabel('时间');
ylabel('估计误差');
legend('状态1估计误差', '状态2估计误差');
title('状态估计误差');

---

 

 

---

 

---