kalman滤波函数matlab

### Kalman 滤波函数在 MATLAB 中的实现

Kalman 滤波是一种用于估计线性和非线性系统的状态的方法，在信号处理领域广泛应用。MATLAB 提供了多种工具来帮助用户理解和应用这一算法。

#### 创建离散时间卡尔曼滤波器对象
为了方便使用，可以创建一个 `kalman` 对象来进行预测和更新操作：

% 定义系统矩阵 A, B 和 C
A = [1 0; 0 1]; % 状态转移模型
B = [0.5; 1];   % 控制输入模型 (如果适用)
C = [1 0];      % 输出测量模型

% 初始化过程噪声 Q 和观测噪声 R 的协方差矩阵
Q = eye(2);     % 过程噪声协方差
R = 1;          % 测量噪声协方差

% 构建 kalman 滤波器对象
sys = ss(A,B,C,0);
[K, L, P] = lqe(sys,Q,R);

kf = kalman(ss(A,[B K],C,[0 D]));

上述代码片段展示了如何定义基本的状态空间模型并初始化 Kalman 滤波器参数[^1]。

#### 预测与校正阶段
一旦有了 Kalman 滤波器实例之后，则可以通过调用其方法完成预测和校正两个主要步骤:

% 假设 u 是控制向量而 y 是当前时刻 t 的实际观察值
u = ... ;       % 输入命令/动作
y = ... ;       % 当前时刻的真实读数

% 执行一步预测
[x_pred, P_pred] = predict(kf, x_prev, P_prev, u);

% 更新基于新的测量数据
[x_est, P_est] = correct(kf, x_pred, P_pred, y);

这里展示的是标准形式下的 Kalman 滤波流程，其中包含了先验估计(`predict`)以及后验修正(`correct`)两部分逻辑[^2]。

#### 示例程序：一维位置跟踪
下面给出了一段完整的例子，它模拟了一个简单的物体在一维直线上的运动情况，并利用 Kalman 滤波对其进行追踪。

function demo_kalman_1D()
    dt = 0.1;
    
    % 动态模型设置
    F = [1 dt];
    H = [1];

    % 协方差设定
    Q = 0.01^2 * eye(length(F));
    R = 0.1^2;

    % 初始条件
    init_state = [0];
    init_covar = diag([0.01]);

    kf = initialize_kalman_filter(F,H,Q,R,init_state,init_covar);

    true_positions = zeros(N,1);
    measurements = zeros(N,1);
    estimates = zeros(N,1);

    for k=1:N
        % Simulate movement and measurement with noise.
        true_pos = simulate_movement(true_positions(k),dt);
        meas = add_measurement_noise(H*true_pos,R);

        % Perform prediction step of the filter.
        [~, pred_p] = predict(kf,[],[],[]);
        
        % Update estimate using new measurement information.
        est = correct(kf,pred_p,meas);
        
        % Store results at this time step.
        true_positions(k) = true_pos;
        measurements(k) = meas;
        estimates(k) = est;
    end
    
    plot_results(true_positions,measurements,estimates);
end

function pos = simulate_movement(pos_last, delta_t)
    v = randn(); % Random velocity change as process noise.
    pos = pos_last + v * delta_t;
end

function z_noisy = add_measurement_noise(z_true, var_z)
    z_noisy = z_true + sqrt(var_z)*randn();
end

function plothandle = plot_results(truths,zs,xhats)
    figure('Name','Kalman Filter Demo');
    hold on;
    grid minor;
    title({'One-Dimensional Position Tracking'; ...
           'True position (blue), Noisy Measurements (green), Estimates (red)'});
    xlabel('Time Step'), ylabel('Position');

    hTruth = stairs(truths,'b-o'); set(hTruth,'LineWidth',2);
    hMeasur = scatter((1:length(zs))',zs,'g.');
    hEstima = stairs(xhats,'r-s'); set(hEstima,'LineWidth',2);

    legend([hTruth,hMeasur,hEstima],'True Positions',...
            'Noisy Measurements','Estimated States')
end

这段脚本通过一系列的时间步长迭代实现了对目标位置变化趋势的学习，并最终绘制出了真实轨迹、带噪观测值及由 Kalman 滤波得出的最佳估算路径之间的对比图谱[^3]。