请用matlab代码实现卡尔曼滤波器
时间: 2023-12-15 22:04:49 浏览: 27
请问这个问题属于技术问题,可以回答。以下是卡尔曼滤波器的MATLAB代码示例:
% 设置初始状态
x = [0;0];
P = [1 0;0 1];
A = [1 1;0 1];
Q = [0.0001 0;0 0.0001];
H = [1 0];
R = 1;
% 生成真实数据z和噪声v
n = 100;
z = zeros(1,n);
x_true = zeros(2,n);
v = randn(1,n)*sqrt(R);
for k = 2:n
x_true(:,k) = A*x_true(:,k-1);
z(k) = H*x_true(:,k) + v(k);
end
% 初始化卡尔曼滤波器
x_hat = [0;0];
P_hat = [1 0;0 1];
% 实现卡尔曼滤波器
for k = 2:n
% 预测
x_hat(:,k) = A*x_hat(:,k-1);
P_hat = A*P_hat*A' + Q;
% 更新
K = P_hat*H'/(H*P_hat*H' + R);
x_hat(:,k) = x_hat(:,k) + K*(z(k) - H*x_hat(:,k));
P_hat = (eye(2) - K*H)*P_hat;
end
% 绘制真实数据、加噪声数据和卡尔曼滤波器结果
figure;
plot(1:n,x_true(1,:),1:n,z,1:n,x_hat(1,:));
legend('真实数据','加噪声数据','卡尔曼滤波器结果');
xlabel('时间');
ylabel('状态值');