matlb代码实现卡尔曼滤波

### MATLAB 实现卡尔曼滤波示例代码

下面展示一段用于实现卡尔曼滤波的MATLAB代码，该代码简洁明了并具有良好的可读性[^1]。

% 初始化变量
n = 50; % 数据长度
t = linspace(0, 1, n);
real_Angle = sin(2 * pi * t); % 假设真实角度随时间变化规律为正弦函数
measure_Angle = real_Angle + 0.1 * randn(size(t)); % 测量数据加入噪声

Q = 1e-5; % 过程噪声协方差矩阵
R = 0.1^2; % 测量噪声协方差矩阵
H = 1;     % 观测模型
I = eye(1);

x_hat = zeros(n, 1); % 状态估计初始化
P = zeros(n, 1);    % 误差协方差初始化
x_hatminus = zeros(n, 1);
Pminus = zeros(n, 1);

x_hat(1) = measure_Angle(1); % 初始状态等于第一次测量值
P(1) = 1;

for k = 2:n
    % 预测更新
    x_hatminus(k) = x_hat(k - 1);
    Pminus(k) = P(k - 1) + Q;
    
    % 测量更新
    K = Pminus(k) / (Pminus(k) + R); % 计算卡尔曼增益
    x_hat(k) = x_hatminus(k) + K * (measure_Angle(k) - H * x_hatminus(k));
    P(k) = (1 - K * H) * Pminus(k);
end

figure;
plot(t, real_Angle, 'b', t, measure_Angle, 'g.', t, x_hat, 'r');
legend('Real Angle', 'Measurements', 'Kalman Filter Estimate');
xlabel('Time(s)');
ylabel('Angle(rad)');
title('Comparison of Real and Estimated Angles using Kalman Filter');

这段程序模拟了一维情况下对物体旋转角位置的跟踪过程。通过设定适当的过程噪声和观测噪声参数`Q`与`R`来调整算法性能，并最终绘制出原始信号、带噪观测以及经过卡尔曼滤波后的平滑曲线以便对比效果。