MATLAB 卡尔曼滤波器设计与信号去噪应用
卡尔曼滤波（Kalman Filter）是一种在时域内基于状态空间模型的最优递推估计算法。它通过融合系统的先验动态模型与带有噪声的传感器观测数据，能够实时计算出系统内部状态的最小方差估计。该算法在导航制导、目标跟踪以及传感器数据融合等领域发挥着核心作用。
核心算法机制
卡尔曼滤波的递推过程主要分为"预测"与"更新"两个阶段，其数学本质是对高斯分布的均值和协方差进行线性变换与贝叶斯融合：
- 先验预测（Time Update）：利用状态转移方程推算当前时刻的先验状态均值与先验误差协方差，反映系统自身演化带来的不确定性增加。
- 卡尔曼增益计算：根据先验协方差与观测噪声协方差的相对大小，动态分配模型预测值与实际测量值之间的权重。
- 后验更新（Measurement Update）：引入最新的观测数据修正先验估计，得到后验状态均值与后验误差协方差，从而降低系统的不确定性。
核心算法的 MATLAB 封装
以下代码重构了标准线性卡尔曼滤波器的核心逻辑。为了提高代码的鲁棒性与可读性，状态协方差序列采用元胞数组（Cell Array）进行存储，避免了多维矩阵索引带来的维度混淆问题。
function [state_est, cov_est] = execute_kalman_filter(obs_data, F, H, Q, R, x_init, P_init)
% 输入参数:
% obs_data : 观测序列 (N x m)，N为采样点数，m为观测维度
% F : 状态转移矩阵 (n x n)
% H : 观测映射矩阵 (m x n)
% Q : 过程激励噪声协方差 (n x n)
% R : 观测噪声协方差 (m x m)
% x_init : 初始状态向量 (n x 1)
% P_init : 初始误差协方差矩阵 (n x n)
%
% 输出参数:
% state_est: 后验状态估计序列 (N x n)
% cov_est : 后验协方差估计序列 (N x 1 元胞数组，每个元素为 n x n 矩阵)
[num_steps, ~] = size(obs_data);
state_dim = length(x_init);
% 预分配内存以提升执行效率
state_est = zeros(num_steps, state_dim);
cov_est = cell(num_steps, 1);
% 设定初始条件
state_est(1, :) = x_init(:)';
cov_est{1} = P_init;
% 递推滤波主循环
for t = 2:num_steps
% 1. 时间更新 (预测)
x_prior = F * state_est(t-1, :)';
P_prior = F * cov_est{t-1} * F' + Q;
% 2. 计算卡尔曼增益
innovation_cov = H * P_prior * H' + R;
K_gain = P_prior * H' / innovation_cov;
% 3. 测量更新 (校正)
innovation = obs_data(t, :)' - H * x_prior;
x_post = x_prior + K_gain * innovation;
P_post = (eye(state_dim) - K_gain * H) * P_prior;
% 保存当前时刻的后验结果
state_est(t, :) = x_post';
cov_est{t} = P_post;
end
end
仿真实验与去噪效果验证
为了验证上述算法的有效性，我们构建一个标量追踪场景。假设目标真实轨迹为带有非线性变化的连续信号，传感器采集的数据叠加了高斯白噪声。通过调整过程噪声协方差与观测噪声协方差的比值，可以控制滤波器对模型和测量的信任程度。
% 场景参数配置
F_mat = 1; % 一维状态转移系数
H_mat = 1; % 一维观测系数
Q_var = 0.05; % 过程噪声方差 (反映模型不确定性)
R_var = 0.8; % 观测噪声方差 (反映传感器精度)
x_start = 0.5; % 状态初始猜测值
P_start = 2.0; % 初始协方差猜测值
% 构造测试数据集
total_steps = 150;
time_vec = linspace(0, 4*pi, total_steps);
% 真实物理信号：叠加了衰减因子的正弦波
ground_truth = exp(-0.1 * time_vec) .* sin(time_vec);
% 模拟传感器带噪观测
sensor_noise = sqrt(R_var) * randn(1, total_steps);
noisy_observations = ground_truth + sensor_noise;
% 执行卡尔曼滤波
[filtered_states, ~] = execute_kalman_filter(...
noisy_observations', F_mat, H_mat, Q_var, R_var, x_start, P_start);
% 可视化对比分析
figure('Name', 'Kalman Filter Denoising', 'Position', [100, 100, 800, 450]);
plot(time_vec, ground_truth, 'k-', 'LineWidth', 2.5, 'DisplayName', 'Ground Truth');
hold on;
scatter(time_vec, noisy_observations, 15, [0.8 0.2 0.2], 'filled', 'MarkerFaceAlpha', 0.4, 'DisplayName', 'Noisy Measurements');
plot(time_vec, filtered_states, 'b--', 'LineWidth', 2, 'DisplayName', 'Kalman Estimate');
% 图表修饰
title('Signal State Estimation and Noise Reduction via Kalman Filter');
xlabel('Time Index');
ylabel('Amplitude');
legend('Location', 'best', 'FontSize', 11);
grid on;
set(gca, 'FontSize', 11);
hold off;
