用MATLAB实现卡尔曼滤波从公式推导到完整代码手把手教你导读卡尔曼滤波Kalman Filter是工程领域应用最广泛的估计算法之一——GPS定位、自动驾驶、无人机姿态估计、雷达跟踪……背后都有它的身影。但很多人面对那堆公式望而却步。今天我们用直觉代码的方式把它彻底讲清楚。一、卡尔曼滤波在解决什么问题一句话概括在有噪声的观测数据中估计出系统的真实状态。举个例子——你有一个GPS传感器在追踪一辆车但GPS数据是有误差的测量值和真实位置之间总存在偏差。卡尔曼滤波做的事情就是真实位置: → ── → ── → ── → ── → ↕ ↕ ↕ 观测数据: → ──·→ ·─→ ·─→ ·─→ ·─→ 带噪声 ↕ ↕ ↕ 卡尔曼估计: → ──→ ──→ ──→ ──→ ──→ 最优估计它通过结合系统模型预测和传感器观测修正得到比任何单一信息源都更准确的估计值。二、核心思想预测 更新卡尔曼滤波的核心就是一个两步循环Step 1预测Predict根据系统模型预测当前时刻的状态“按照物理规律物体应该在这。”Step 2更新Update用传感器观测值修正预测结果“但传感器说它在另一个位置综合一下我觉得它在两者之间更偏向误差更小的那个。”一个直觉类比想象你在追一个朋友的位置预测你知道他之前在走、大概速度多少所以你猜他现在应该在前方50米处更新但你的GPS说他实际在前方60米处融合你对他的运动模型不是100%确定对GPS也不是100%确定所以你综合两者估计他大约在前方55米这就是卡尔曼滤波的本质——在不确定中寻找最优估计。三、数学公式只保留核心5个别怕我们用直觉来解释每个公式。状态空间模型状态方程系统怎么演化x_k F * x_{k-1} B * u_k w_kx_k当前时刻的系统状态如位置、速度F状态转移矩阵描述状态如何随时间变化B * u_k控制输入如加速度w_k过程噪声模型不完美带来的误差观测方程传感器能测到什么z_k H * x_k v_kz_k观测值传感器读数H观测矩阵状态到观测的映射关系v_k观测噪声传感器本身的误差预测步骤x̂_k|k-1 F * x̂_{k-1} B * u_k ← 状态预测用模型猜下一步在哪 P_k|k-1 F * P_{k-1} * F Q ← 协方差预测预测的不确定性有多大P估计误差协方差矩阵衡量估计的不确定程度Q过程噪声协方差模型有多不靠谱更新步骤K_k P_k|k-1 * H * (H * P_k|k-1 * H R)^(-1) ← 卡尔曼增益该信谁 x̂_k x̂_k|k-1 K_k * (z_k - H * x̂_k|k-1) ← 状态更新融合预测和观测 P_k (I - K_k * H) * P_k|k-1 ← 协方差更新更新后的不确定性K卡尔曼增益——最核心的量。它决定了更信预测还是更信观测K 大 → 更信观测传感器可靠K 小 → 更信预测模型可靠R观测噪声协方差传感器有多不靠谱四、MATLAB完整实现下面我们用一个匀速直线运动的追踪问题来演示完整代码。问题设定目标沿一维匀速运动状态 [位置, 速度]传感器只观测位置带高斯噪声用卡尔曼滤波估计真实位置和速度%% % 卡尔曼滤波 MATLAB 完整实现% 场景一维匀速运动目标追踪% MATLAB版本R2020a及以上% 依赖工具箱无%% clear;clc;close all;%% 1. 仿真参数设置dt0.1;% 时间步长秒T20;% 仿真总时长秒NT/dt;% 总步数t(0:N-1)*dt;% 时间序列%% 2. 生成真实轨迹% 真实初始状态[位置(米), 速度(米/秒)]true_statezeros(2,N);true_state(1,1)0;% 初始位置 0mtrue_state(2,1)2;% 初始速度 2m/s匀速运动% 真实运动x_{k1} F * x_k 过程噪声F[1,dt;% 状态转移矩阵0,1];Q[0.1,0;% 过程噪声协方差模型不完美的程度0,0.1];% Q值越大说明模型越不靠谱滤波器越依赖观测rng(42);% 设置随机种子保证可复现fork2:Ntrue_state(:,k)F*true_state(:,k-1)...mvnrnd([0;0],Q);end%% 3. 生成带噪声的观测数据% 观测矩阵只能观测到位置不能直接观测速度H[1,0];R4;% 观测噪声方差传感器精度越大越不准% 观测值 真实位置 高斯噪声measurementsH*true_statesqrt(R)*randn(1,N);%% 4. 卡尔曼滤波估计% 初始估计x_estzeros(2,N);% 状态估计P_estzeros(2,2,N);% 误差协方差x_est(:,1)[0;0];% 初始状态估计假设不知道真实值P_est(:,:,1)[1,0;% 初始协方差表示初始估计不确定0,1];fork2:N%% --- 预测步骤 ---% 状态预测x_predF*x_est(:,k-1);% 协方差预测P_predF*P_est(:,:,k-1)*FQ;%% --- 更新步骤 ---% 计算卡尔曼增益SH*P_pred*HR;% 新息协方差KP_pred*H/S;% 卡尔曼增益标量% 状态更新预测 增益 × 残差innovationmeasurements(k)-H*x_pred;% 新息残差x_est(:,k)x_predK*innovation;% 协方差更新P_est(:,:,k)(eye(2)-K*H)*P_pred;end%% 5. 结果可视化figure(Position,[100,100,900,600]);% 子图1位置估计subplot(2,1,1);plot(t,true_state(1,:),k-,LineWidth,1.5);hold on;plot(t,measurements,r.,MarkerSize,4);plot(t,x_est(1,:),b--,LineWidth,1.5);xlabel(时间 (s),FontSize,12);ylabel(位置 (m),FontSize,12);title(卡尔曼滤波 — 位置估计,FontSize,14);legend(真实位置,观测数据含噪声,卡尔曼估计,...Location,northwest);grid on;xlim([0,T]);% 子图2估计误差subplot(2,1,2);pos_errorx_est(1,:)-true_state(1,:);% 卡尔曼估计误差obs_errormeasurements-true_state(1,:);% 观测误差plot(t,obs_error,r-,LineWidth,0.8);hold on;plot(t,pos_error,b-,LineWidth,1.2);xlabel(时间 (s),FontSize,12);ylabel(误差 (m),FontSize,12);title(估计误差对比,FontSize,14);legend(观测误差,卡尔曼估计误差,Location,northeast);grid on;xlim([0,T]);% 添加误差统计信息text(0.02,0.85,...sprintf(观测误差 RMSE: %.2f m\n卡尔曼误差 RMSE: %.2f m,...rms(obs_error),rms(pos_error)),...Units,normalized,FontSize,11,...BackgroundColor,w);%% 6. 误差统计输出fprintf( 卡尔曼滤波性能统计 \n);fprintf(观测误差 RMSE: %.4f m\n,rms(obs_error));fprintf(卡尔曼估计 RMSE: %.4f m\n,rms(pos_error));fprintf(误差降低: %.1f%%\n,...(1-rms(pos_error)/rms(obs_error))*100);运行结果运行上述代码你会得到类似下面的结果 卡尔曼滤波性能统计 观测误差 RMSE: 2.0012 m 卡尔曼估计 RMSE: 0.4823 m 误差降低: 75.9%卡尔曼滤波将观测误差降低了约76%。仿真结果图展示了黑色实线真实位置轨迹红色散点带噪声的GPS观测值明显波动蓝色虚线卡尔曼滤波的估计值非常贴近真实值五、关键要点总结1. Q 和 R 的调参是核心Q 大 → 不信任模型 → 更依赖观测 → 响应快但噪声多 Q 小 → 信任模型 → 更依赖预测 → 平滑但响应慢 R 大 → 不信任传感器 → 更依赖预测 R 小 → 信任传感器 → 更依赖观测实际调参建议先根据传感器参数手册设定 R然后逐步调整 Q 直到滤波效果满意。2. 初始化的影响初始状态x̂₀不需要很准卡尔曼滤波会快速收敛初始协方差P₀设大一些表示我承认自己初始估计不准3. 卡尔曼滤波的适用条件条件说明线性系统状态方程和观测方程是线性的高斯噪声过程噪声和观测噪声服从高斯分布已知噪声参数需要知道或估计Q和R的值如果系统是非线性的需要使用扩展卡尔曼滤波EKF或无迹卡尔曼滤波UKF后续文章会专门讲解。六、扩展阅读扩展卡尔曼滤波EKF处理非线性系统的经典方法无迹卡尔曼滤波UKF比EKF更稳定不需要计算雅可比矩阵粒子滤波处理任意非线性和非高斯系统下期预告觉得有帮助点赞 在看 转发让更多同行看到。有问题欢迎评论区交流