matlab实现kf代码

以下是一个简单的MATLAB代码实现卡尔曼滤波（KF）的示例：

% KF参数设置
A = [1 1; 0 1];  % 状态转移矩阵
C = [1 0];      % 观测矩阵
Q = [0.1 0; 0 0.1];  % 过程噪声的协方差矩阵
R = 1;  % 观测噪声的协方差

% 初始化
x_hat = [0; 0];  % 初始状态估计值
P = eye(2);      % 初始状态协方差估计矩阵

% 数据生成
T = 100;  % 时间步长
x_true = zeros(2, T);  % 真实状态值
z = zeros(1, T);       % 观测值
for t = 1:T
    x_true(:, t) = A * x_hat + mvnrnd([0 0], Q)';  % 生成真实状态值
    z(t) = C * x_true(:, t) + normrnd(0, sqrt(R));  % 生成观测值
end

% KF迭代更新
x_estimated = zeros(2, T);  % 状态估计序列
for t = 1:T
    % 预测步骤
    x_hat = A * x_hat;
    P = A * P * A' + Q;

    % 更新步骤
    y = z(t) - C * x_hat;  % 观测残差
    S = C * P * C' + R;    % 观测残差的协方差矩阵
    K = P * C' / S;        % 卡尔曼增益
    x_hat = x_hat + K * y;  % 更新状态估计值
    P = (eye(2) - K * C) * P;  % 更新状态协方差估计矩阵

    x_estimated(:, t) = x_hat;  % 保存状态估计值
end

% 绘制结果
figure;
subplot(2, 1, 1);
plot(1:T, x_true(1, :), 'b', 1:T, x_estimated(1, :), 'r--');
legend('真实状态值', '状态估计值');
xlabel('时间步长');
ylabel('状态值');
title('状态值估计');

subplot(2, 1, 2);
plot(1:T, z, 'k', 1:T, C * x_estimated, 'r--');
legend('观测值', '观测值估计');
xlabel('时间步长');
ylabel('观测值');
title('观测值估计');

这段代码实现了一个简单的一维卡尔曼滤波器。它首先生成一个带有噪声的状态序列和相应的观测序列，然后使用KF算法对观测序列进行滤波和估计，并将结果绘制出来。你可以根据自己的需求修改代码中的参数和数据生成部分。