卡尔曼滤波例题 matlab代码

以下是一个简单的卡尔曼滤波器的例子，使用MATLAB编写：

% 状态转移矩阵
A = [1 1; 0 1];

% 观测矩阵
C = [1 0];

% 系统噪声方差
Q = 0.01;

% 观测噪声方差
R = 1;

% 初始状态估计
x0 = [0; 0];

% 初始协方差矩阵
P0 = eye(2);

% 生成真实状态和观测数据
T = 100;
x_true = zeros(2, T);
y = zeros(1, T);

x_true(:, 1) = mvnrnd(x0, P0)';
y(1) = C*x_true(:, 1) + sqrt(R)*randn();

for t = 2:T
    x_true(:, t) = A*x_true(:, t-1) + sqrt(Q)*randn();
    y(t) = C*x_true(:, t) + sqrt(R)*randn();
end

% 卡尔曼滤波
x_est = zeros(2, T);
P_est = zeros(2, 2, T);
K = zeros(2, T);

x_est(:, 1) = x0;
P_est(:, :, 1) = P0;

for t = 2:T
    % 预测步骤
    x_pred = A*x_est(:, t-1);
    P_pred = A*P_est(:, :, t-1)*A' + Q;
    
    % 更新步骤
    K(:, t) = P_pred*C'/(C*P_pred*C' + R);
    x_est(:, t) = x_pred + K(:, t)*(y(t) - C*x_pred);
    P_est(:, :, t) = (eye(2) - K(:, t)*C)*P_pred;
end

% 绘制结果
figure;
subplot(2, 1, 1);
plot(1:T, x_true(1, :), 'b', 1:T, x_est(1, :), 'r');
legend('真实状态', '滤波估计');
ylabel('状态1');

subplot(2, 1, 2);
plot(1:T, x_true(2, :), 'b', 1:T, x_est(2, :), 'r');
legend('真实状态', '滤波估计');
ylabel('状态2');

figure;
plot(1:T, y, 'b', 1:T, C*x_est, 'r');
legend('观测数据', '滤波估计');
ylabel('观测值');

这个例子演示了如何使用卡尔曼滤波器来估计线性系统的状态。首先，定义了系统的状态转移矩阵A和观测矩阵C，以及系统噪声方差Q和观测噪声方差R。然后，生成了一些带有噪声的真实状态和观测数据。接下来，通过卡尔曼滤波器进行状态估计，包括预测步骤和更新步骤。最后，绘制了真实状态、滤波估计和观测数据的图形。