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

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

代码框架：

左上角：带误差棒或离散点的趋势图
这看起来像是某种随时间或序列变化的指标，包含两组数据对比。

import matplotlib.pyplot as plt
import numpy as np

模拟数据
x = np.arange(0, 90, 5)
y1 = np.random.randint(0, 40, size=len(x)) # 绿色数据
y2 = np.random.randint(0, 10, size=len(x)) # 红色数据

plt.figure(figsize=(6, 4))
绘制绿色折线和点
plt.plot(x, y1, ‘g-o’, label=‘数据组1’, markersize=4)
绘制红色折线和点
plt.plot(x, y2, ‘r-s’, label=‘数据组2’, markersize=4)

plt.legend()
plt.title(“趋势对比图示例”)
plt.xlabel(“样本序列”)
plt.ylabel(“数值”)
plt.grid(True, alpha=0.3)
plt.show()

右上角：卡尔曼滤波仿真
这是一个典型的卡尔曼滤波效果图，展示了真实值、测量值和滤波后的估计值。

import matplotlib.pyplot as plt
import numpy as np

模拟卡尔曼滤波数据
dt = 0.1
t = 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-’, alpha=0.5, label=‘测量值’)
plt.plot(t, real_val, ‘g-’, linewidth=2, label=‘真实值’)
plt.plot(t, kalman_val, ‘b-’, linewidth=2, label=‘卡尔曼估计’)

plt.legend()
plt.title(“Kalman Filter Simulation”)
plt.xlabel(“Sample Time”)
plt.ylabel(“Temperature/C”)
plt.show()

左下角：平滑曲线拟合
这是一个带有峰值的曲线，显示了原始数据和拟合曲线（如高斯拟合或多项式拟合）。

import matplotlib.pyplot as plt
import numpy as np
from 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, s=10, color=‘k’, label=‘原始数据’)
plt.plot(x, p(x), “r-”, linewidth=2, label=“拟合曲线”)

plt.legend()
plt.title(“Figure 8”)
plt.xlabel(“x方向位置/米”)
plt.ylabel(“高程/米”)
plt.show()

右下角：周期性波动对比
展示了多条曲线在同一坐标系下的周期性变化。

import matplotlib.pyplot as plt
import 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, alpha=0.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），这证明了滤波器的有效性。