用matlab写一个卡尔曼滤波器
时间: 2023-11-21 17:21:11 浏览: 28
以下是一个简单的示例,用MATLAB实现卡尔曼滤波器:
% 定义系统模型
A = [1 1; 0 1];
B = [0.5; 1];
C = [1 0];
% 定义系统参数
Q = [0.001 0; 0 0.01];
R = 0.1;
x0 = [0; 0];
P0 = [10 0; 0 10];
% 生成随机信号
u = randn(100, 1);
v = sqrt(R) * randn(100, 1);
% 初始化
x_hat = zeros(2, 100);
x_hat(:, 1) = x0;
P = zeros(2, 2, 100);
P(:, :, 1) = P0;
% 卡尔曼滤波
for k = 2:100
% 预测
x_hat(:, k) = A * x_hat(:, k-1) + B * u(k-1);
P(:, :, k) = A * P(:, :, k-1) * A' + Q;
% 更新
K = P(:, :, k) * C' / (C * P(:, :, k) * C' + R);
x_hat(:, k) = x_hat(:, k) + K * (v(k) - C * x_hat(:, k));
P(:, :, k) = (eye(2) - K * C) * P(:, :, k);
end
% 绘制结果
t = 1:100;
figure;
subplot(2, 1, 1);
plot(t, x_hat(1, :));
title('State 1');
subplot(2, 1, 2);
plot(t, x_hat(2, :));
title('State 2');