卡尔曼滤波Matlab程序 kalman KALMAN

卡尔曼滤波Matlab程序 kalman KALMAN
处理力传感器的信号，从csv文件中读取数据并且进行卡尔曼滤波，滤波前后得数据进行对比。

2. 完整代码

%% 1. 读取数据
fileName = 'force_data.csv';          % CSV 文件名
rawData  = readmatrix(fileName);      % 读取一列
fs       = 1000;                      % 采样率 (Hz)，请按实际修改
t        = (0:length(rawData)-1)/fs;  % 时间轴

%% 2. 设计卡尔曼滤波器
% 系统模型：x(k+1) = A x(k) + w(k)   （随机游走）
%           y(k)   = C x(k) + v(k)
A  = 1;               % 状态转移
C  = 1;               % 观测矩阵
Q  = 1e-4;            % 过程噪声协方差（调参）
R  = 1e-2;            % 观测噪声协方差（调参）

% 初始化
x_hat     = 0;        % 状态估计初值
P         = 1;        % 误差协方差初值
nSamples  = numel(rawData);
filtered  = zeros(nSamples,1);

for k = 1:nSamples
    % 1) 预测
    x_pred = A * x_hat;
    P_pred = A * P * A' + Q;
    
    % 2) 更新
    K      = P_pred * C' / (C * P_pred * C' + R);   % 卡尔曼增益
    x_hat  = x_pred + K * (rawData(k) - C * x_pred);
    P      = (1 - K * C) * P_pred;
    
    filtered(k) = x_hat;
end

%% 3. 结果可视化
figure('Color','w');
subplot(2,1,1);
plot(t, rawData, 'b', 'LineWidth', 0.8); hold on;
plot(t, filtered, 'r', 'LineWidth', 1.2);
legend('原始信号','卡尔曼滤波'); grid on;
title('力传感器信号对比'); xlabel('时间 (s)'); ylabel('力');

subplot(2,1,2);
plot(t, rawData - filtered, 'k');
grid on;
title('残差 (原始 - 滤波)');
xlabel('时间 (s)'); ylabel('力');

%% 4. 保存滤波后数据（可选）
writematrix(filtered, 'force_filtered.csv');