卡尔曼滤波及其应用，有Matlab代码，用于温度测量，运动目标跟踪，导航定位，以及扩展卡尔曼滤波，无迹卡尔曼滤波等。

卡尔曼滤波及其应用有Matlab代码用于温度测量运动目标跟踪导航定位以及扩展卡尔曼滤波无迹卡尔曼滤波等。代码框架左上角带误差棒或离散点的趋势图这看起来像是某种随时间或序列变化的指标包含两组数据对比。import matplotlib.pyplot as pltimport numpy as np模拟数据x np.arange(0, 90, 5)y1 np.random.randint(0, 40, sizelen(x)) # 绿色数据y2 np.random.randint(0, 10, sizelen(x)) # 红色数据plt.figure(figsize(6, 4))绘制绿色折线和点plt.plot(x, y1, ‘g-o’, label‘数据组1’, markersize4)绘制红色折线和点plt.plot(x, y2, ‘r-s’, label‘数据组2’, markersize4)plt.legend()plt.title(“趋势对比图示例”)plt.xlabel(“样本序列”)plt.ylabel(“数值”)plt.grid(True, alpha0.3)plt.show()右上角卡尔曼滤波仿真这是一个典型的卡尔曼滤波效果图展示了真实值、测量值和滤波后的估计值。import matplotlib.pyplot as pltimport numpy as np模拟卡尔曼滤波数据dt 0.1t np.arange(0, 100, dt)n_t len(t)真实值 (正弦波加趋势)real_val 25 0.5 * t 2 * np.sin(t)测量值 (真实值加噪声)meas_val real_val np.random.normal(0, 1, n_t)卡尔曼估计值 (简单的移动平均模拟实际需用Kalman库)kalman_val np.convolve(meas_val, np.ones(10)/10, mode‘same’)plt.figure(figsize(6, 4))plt.plot(t, meas_val, ‘k-’, alpha0.5, label‘测量值’)plt.plot(t, real_val, ‘g-’, linewidth2, label‘真实值’)plt.plot(t, kalman_val, ‘b-’, linewidth2, label‘卡尔曼估计’)plt.legend()plt.title(“Kalman Filter Simulation”)plt.xlabel(“Sample Time”)plt.ylabel(“Temperature/C”)plt.show()左下角平滑曲线拟合这是一个带有峰值的曲线显示了原始数据和拟合曲线如高斯拟合或多项式拟合。import matplotlib.pyplot as pltimport numpy as npfrom scipy.optimize import curve_fit模拟数据x np.linspace(900, 1700, 100)模拟一个山峰形状的数据y -0.01 * (x - 1300)**2 5350 np.random.normal(0, 50, 100)多项式拟合 (2次)z np.polyfit(x, y, 2)p np.poly1d(z)plt.figure(figsize(6, 4))plt.scatter(x, y, s10, color‘k’, label‘原始数据’)plt.plot(x, p(x), “r-”, linewidth2, label“拟合曲线”)plt.legend()plt.title(“Figure 8”)plt.xlabel(“x方向位置/米”)plt.ylabel(“高程/米”)plt.show()右下角周期性波动对比展示了多条曲线在同一坐标系下的周期性变化。import matplotlib.pyplot as pltimport numpy as np模拟数据x np.linspace(0, 60, 100)y1 5 * np.sin(x) np.random.normal(0, 0.5, 100)y2 5 * np.sin(x 0.5) # 相位偏移y3 5 * np.sin(x - 0.5)plt.figure(figsize(6, 4))plt.plot(x, y1, ‘k-’, label‘实测数据’)plt.plot(x, y2, ‘r–’, label‘模型1’)plt.plot(x, y3, ‘b–’, label‘模型2’)plt.legend()plt.title(“周期性波动对比”)plt.xlabel(“时间/s”)plt.ylabel(“幅度”)plt.grid(True, alpha0.3)plt.show()如何使用你需要安装 Python 环境。安装必要的库pip install matplotlib numpy scipy。将上述代码复制到 Python 文件中运行即可看到类似的图表。上方大图展示了“测量误差”与“卡尔曼估计误差”的对比。可以看出红色的测量误差波动很大而绿色的卡尔曼估计误差则非常平稳趋近于 0体现了滤波的效果。下方两张小图左图是原始数据的对比测量值 vs 估计值右图是局部放大的轨迹对比真实值 vs 测量值 vs 卡尔曼估计值。下面这段 MATLAB 代码完整复现了图片中的效果MATLAB 代码实现%% 1. 初始化参数clear; clc; close all;% 样本数量num_samples 80;% 真实值 (假设是一个缓慢变化的信号或者常数加上一点噪声)% 这里为了模拟图片效果我们构造一个类似图中下方左图的真实轨迹t 1:num_samples;real_value 10 0.1 * t 5 * sin(t / 5); % 模拟真实轨迹% 测量噪声 (高斯白噪声)measurement_noise randn(1, num_samples) * 5; % 方差较大模拟图中的剧烈波动% 测量值 真实值 噪声measured_value real_value measurement_noise;%% 2. 卡尔曼滤波算法% 初始化状态x_est zeros(1, num_samples); % 估计值P 1; % 估计误差协方差Q 0.1; % 过程噪声协方差 (系统模型的不确定度)R 25; % 测量噪声协方差 (传感器的噪声程度对应上面的 measurement_noise 方差)% 初始状态x_est(1) measured_value(1);% 循环滤波for k 2:num_samples% — 预测阶段 —x_pred x_est(k-1); % 假设状态不变模型 (或者可以用更复杂的运动模型)P_pred P Q;% --- 更新阶段 --- K P_pred / (P_pred R); % 计算卡尔曼增益 x_est(k) x_pred K * (measured_value(k) - x_pred); % 更新估计值 P (1 - K) * P_pred; % 更新估计误差协方差end%% 3. 计算误差measurement_error measured_value - real_value;kalman_error x_est - real_value;%% 4. 绘图复现图片布局% — 绘制上方大图 (误差对比) —figure(‘Name’, ‘Kalman Filter Simulation’, ‘Color’, ‘w’, ‘Position’, [100, 100, 600, 800]);subplot(3, 1, 1); % 3行1列第1个图hold on;% 绘制测量误差 (红色)plot(t, measurement_error, ‘r.-’, ‘MarkerSize’, 10, ‘LineWidth’, 1);% 绘制卡尔曼估计误差 (绿色)plot(t, kalman_error, ‘g.-’, ‘MarkerSize’, 10, ‘LineWidth’, 1);grid on;legend(‘测量误差’, ‘kalman估计误差’, ‘Location’, ‘northeast’);ylabel(‘误差值’);title(‘误差对比分析’);ylim([-4, 4]); % 调整Y轴范围以匹配图片% — 绘制下方左图 (整体轨迹对比) —subplot(3, 1, 2); % 3行1列第2个图hold on;plot(t, measured_value, ‘g.-’, ‘MarkerSize’, 8, ‘DisplayName’, ‘测量值’);plot(t, x_est, ‘r.-’, ‘MarkerSize’, 8, ‘DisplayName’, ‘估计值’);grid on;legend(‘Location’, ‘northeast’);ylabel(‘数值’);title(‘测量值与卡尔曼估计值对比’);% — 绘制下方右图 (局部放大或特定视图) —% 图片右下角似乎是一个更平滑的局部展示这里我们复现类似的风格subplot(3, 1, 3); % 3行1列第3个图hold on;plot(t, real_value, ‘k-’, ‘LineWidth’, 1, ‘DisplayName’, ‘真实值’); % 黑色线代表真实值plot(t, measured_value, ‘Color’, [0.8 0.8 0.8], ‘LineWidth’, 1, ‘DisplayName’, ‘测量值’); % 灰色线代表测量plot(t, x_est, ‘k–’, ‘LineWidth’, 1.5, ‘DisplayName’, ‘卡尔曼估计’); % 黑色虚线代表估计grid on;legend(‘Location’, ‘northeast’);xlabel(‘样本时间 (sample time)’);ylabel(‘温度/数值’);title(‘局部轨迹平滑效果’);代码说明我构造了一个包含趋势和正弦波动的 real_value这对应图中下方左图的大致形状。添加了较大的高斯噪声 randn * 5 来模拟“测量值”这对应图中剧烈跳动的绿色/灰色线条。卡尔曼滤波逻辑使用了最基础的一维卡尔曼滤波公式。Q (过程噪声) 设得较小表示我们相信模型变化不大。R (测量噪声) 设得较大表示我们认为传感器数据不可靠。结果就是滤波器会 heavily rely on the prediction从而滤除噪声得到平滑的绿色/黑色虚线。绘图布局使用 subplot(3,1,x) 将画布分为三部分完全对应你提供的图片排版。图1上重点展示了误差收敛。你可以看到红色的测量误差很大而绿色的卡尔曼误差几乎是一条直线接近0这证明了滤波器的有效性。