从零开始写卡尔曼滤波Matlab代码——这篇文章让你不再懵逼！

你有没有遇到过这种情况？用传感器测数据的时候，数值跳来跳去，根本没法看真实值？比如手机的加速度传感器走路时晃得厉害，或者无人机的姿态估计噪声大到没法用？这时候卡尔曼滤波就能帮你一把！它就像一个智能过滤器，能从一堆噪声里挖出最接近真实的那个值！今天我就带你从零开始写卡尔曼滤波的Matlab代码，保证你看完就能跑起来！

一、先搞懂卡尔曼滤波的核心逻辑

卡尔曼滤波其实就两步：预测和更新。是不是超简单？ - 预测：根据上一时刻的状态，猜现在的状态（比如根据昨天的天气猜今天的天气） - 更新：用新的观测值修正这个猜测（比如今天出门看了一眼天空，调整对天气的判断）

举个栗子：你开车时，根据当前速度和方向预测下一秒的位置（预测），然后看一眼GPS发现和预测有点偏差，就调整位置估计（更新）。对，就是这么个逻辑！

二、准备系统模型（超级重要！）

要写代码，得先定义系统模型。我们用直线运动的小车做例子： - 状态向量：位置x + 速度v → X = [x; v] - 状态方程：X(k+1) = A*X(k) + w(k)（w是过程噪声，比如路面颠簸导致的速度变化） - 观测方程：Z(k) = H*X(k) + v(k)（v是观测噪声，比如GPS的测量误差）

这里的关键矩阵得记牢： - 状态转移矩阵A：A = [1 dt; 0 1]（dt是时间步长，比如0.1秒） - 观测矩阵H：H = [1 0]（假设我们只观测位置）

三、Matlab代码分步实现

话不多说，直接上代码！每一步我都加了超详细的注释，保证你看得懂！

1. 定义基础参数

```matlab clear all; close all; clc;

% 时间参数 dt = 0.1; % 时间步长（秒） T = 10; % 总时间（秒） t = 0:dt:T; % 时间向量 n = length(t); % 数据点数量 ```

2. 定义系统矩阵与噪声参数

matlab % 状态转移矩阵A（位置x和速度v的更新） A = [1 dt; 0 1]; % 观测矩阵H（只观测位置x） H = [1 0]; % 过程噪声协方差Q（加速度噪声导致的误差，假设加速度方差0.5） Q = dt^4/4 * 0.5^2 * [1 0;0 0] + dt^2/2 *0.5^2 * [0 0;0 1]; % 观测噪声协方差R（GPS测量噪声，假设方差1） R = 1; 注意！ Q和R的取值直接影响滤波效果！如果Q太大，滤波会太相信观测（噪声还是大）；如果R太大，滤波会太相信预测（反应太慢）。一定要根据实际情况调！我刚开始学的时候，这里踩了好多坑！

3. 生成真实数据与带噪声的观测数据

我们先模拟真实的小车运动，再给观测数据加噪声（模拟实际传感器的情况）： ```matlab % 生成真实状态（假设小车做匀加速运动，加速度a=1） x_true = 0.5 * 1 * t.^2; % 真实位置 v_true = 1 * t; % 真实速度 X_true = [x_true; v_true]; % 真实状态向量

% 生成带噪声的观测数据（模拟GPS测量） z_meas = x_true + sqrt(R)*randn(1,n); ```

4. 初始化卡尔曼滤波参数

```matlab % 存储滤波结果的变量 X_est = zeros(2,n); % 滤波后的状态（位置+速度） P_est = zeros(2,2,n); % 滤波后的协方差矩阵（代表估计的不确定性）

% 初始状态（假设我们知道小车初始位置0，速度0） X_est(:,1) = [0; 0]; % 初始协方差（代表对初始状态的不确定性，数值越大越不确定） P_est(:,:,1) = [1 0; 0 1]; ```

5. 核心滤波循环（预测+更新）

这部分是卡尔曼滤波的灵魂！一定要仔细看每一行： ```matlab for k = 2:n % ------------------- 预测阶段 ------------------- % 预测当前状态 X_pred = A * X_est(:,k-1); % 预测协方差矩阵 P_pred = A * P_est(:,:,k-1) * A' + Q;

end ``` 卡尔曼增益K是关键！它的大小决定了我们有多相信观测值：K越大，越相信观测；K越小，越相信预测。

6. 画图看效果！

最后一步，把真实值、观测值、滤波值放一起对比，看看滤波效果： ```matlab figure; % 位置对比图 subplot(2,1,1); plot(t, x_true, 'b-', 'LineWidth',2); hold on; plot(t, z_meas, 'r.', 'MarkerSize',10); hold on; plot(t, X_est(1,:), 'g--', 'LineWidth',2); xlabel('时间 (s)'); ylabel('位置 (m)'); legend('真实位置','观测位置','滤波位置'); title('卡尔曼滤波位置估计结果');

% 速度对比图 subplot(2,1,2); plot(t, v_true, 'b-', 'LineWidth',2); hold on; plot(t, X_est(2,:), 'g--', 'LineWidth',2); xlabel('时间 (s)'); ylabel('速度 (m/s)'); legend('真实速度','滤波速度'); title('卡尔曼滤波速度估计结果'); ```

四、跑代码看效果！

把上面的代码复制到Matlab里运行，你会看到： - 红色的观测值跳来跳去（噪声大） - 绿色的滤波值平滑地跟着蓝色的真实值走（效果超棒！）

我第一次跑成功的时候，差点喊出来——原来卡尔曼滤波这么神奇！

五、踩坑总结（超级实用！）

1. Q和R的取值：别随便填！要根据传感器的噪声特性来调。比如GPS噪声大，R就设大一点；小车运动颠簸，Q就设大一点。
2. 系统模型要准确：如果你的系统不是直线运动，得改A矩阵哦！比如三维空间运动，A矩阵要变成3维的。
3. 初始状态的选择：如果不知道初始状态，可以把初始协方差P0设大一点，让滤波自己慢慢收敛。

六、最后说两句

卡尔曼滤波的应用真的超广！无人机定位、机器人导航、金融数据预测……只要你需要从噪声中找真实值，它就能派上用场。今天我们写的是最基础的线性卡尔曼滤波，还有扩展卡尔曼滤波（EKF）、无迹卡尔曼滤波（UKF）等进阶版本，感兴趣的话可以自己去查资料！

怎么样？是不是觉得卡尔曼滤波没那么难？赶紧把代码复制到Matlab里跑一遍，感受一下它的魔力吧！如果有问题，欢迎在评论区留言哦（虽然这篇文章没有评论区，但你可以自己琢磨！）。

下次再见啦！祝你学习愉快！