卡尔曼滤波matlab代码练习
时间: 2023-11-06 07:55:03 浏览: 43
你可以使用以下代码来练习卡尔曼滤波的MATLAB实现:
```matlab
% 状态转移矩阵
A = [1 1; 0 1];
% 观测矩阵
H = [1 0];
% 过程噪声方差
Q = [0.01 0; 0 0.01];
% 观测噪声方差
R = 1;
% 初始状态估计
x0 = [0; 0];
% 初始状态协方差矩阵
P0 = [1 0; 0 1];
% 生成一些模拟数据
dt = 0.1;
t = 0:dt:10;
N = length(t);
x_true = [sin(t); cos(t)];
x_obs = x_true + sqrt(R)*randn(size(x_true));
% 初始化变量
x_est = zeros(size(x_true));
P_est = zeros(size(P0));
% 卡尔曼滤波循环
for k = 1:N
% 预测步骤
x_pred = A * x_est(:,k);
P_pred = A * P_est(:,:,k) * A' + Q;
% 更新步骤
K = P_pred * H' / (H * P_pred * H' + R);
x_est(:,k+1) = x_pred + K * (x_obs(:,k) - H * x_pred);
P_est(:,:,k+1) = (eye(size(K * H)) - K * H) * P_pred;
end
% 绘制结果
figure;
plot(t, x_true(1,:), 'r', t, x_obs(1,:), 'g', t, x_est(1,:), 'b');
xlabel('时间');
ylabel('状态变量');
legend('真实值', '观测值', '估计值');
```