卡尔曼滤波去噪MATLAB代码

卡尔曼滤波是一种常用的信号处理技术，尤其适用于线性动态系统的状态估计和噪声削弱。在MATLAB中，实现卡尔曼滤波的基本步骤包括初始化、预测和更新等阶段。这里是一个简化的示例代码：

% 初始化参数
A = % 系统动态矩阵
B = % 控制输入矩阵
H = % 测量矩阵
Q = % 状态方差矩阵
R = % 测量噪声方差矩阵
P0 = % 初始状态协方差矩阵
x0 = % 初始状态向量

function [x, P] = kalmanFilter(x_k_minus_1, P_k_minus_1, u, z)
    % 预测阶段
    x_pred = A * x_k_minus_1;
    P_pred = A * P_k_minus_1 * A' + Q;

    % 更新阶段
    K = P_pred * H' / (H * P_pred * H' + R);
    x = x_pred + K * (z - H * x_pred);
    P = (eye(size(P_pred)) - K * H) * P_pred;

    % 返回当前状态和协方差矩阵
    x = x';
    P = P';
end

% 主循环
for t = 1:length(z) % 假设z是测量数据
    % 获取当前状态和预测误差
    x_k_minus_1 = x(t-1);
    P_k_minus_1 = P(t-1);

    % 调用kalmanFilter函数，u可以为空（无控制输入）
    [x_k, P_k] = kalmanFilter(x_k_minus_1, P_k_minus_1, u(t), z(t));

    % 输出当前状态
    disp(['Time step ', num2str(t), ': State is ', num2str(x_k)]);
    % 或者你可以保存到数组中用于后续分析
    x(:,t) = x_k;
    P(:,t) = P_k;
end