卡尔曼滤波器平滑加速度 matlab

以下是使用Matlab实现卡尔曼滤波器平滑加速度的示例代码：

% 定义观察数据的速度和加速度
velocity = [1.2, 1.5, 1.7, 2.0, 2.2];
acceleration = [0.1, 0.3, 0.5, 0.7, 0.9% 定义状态转移矩阵A和观测矩阵H
A = [1 1; 0 1];
H = [1 0];

% 定义过程噪声协方差Q和观测噪声协方差R
Q = [0.1 0; 0 0.1];
R = 1;

% 定义初始状态和协方差矩阵P
x0 = [0; 0];
P0 = [1 0; 0 1];

% 使用卡尔曼滤波器进行平滑
[x_smooth, P_smooth] = kalman_filter(velocity, acceleration, A, H, Q, R, x0, P0);

% 输出平滑后的加速度
disp(x_smooth(2,:));

% 定义卡尔曼滤波器函数
function [x_smooth, P_smooth] = kalman_filter(y, u, A, H, Q, R, x0, P0)
    % 初始化
    n = size(y, 2);
    x_smooth = zeros(size(x0, 1), n);
    P_smooth = zeros(size(P0, 1), size(P0, 2), n);
    x_pred = x0;
    P_pred = P0;
    
    % 递推
    for i = 1:n
        % 预测
        x_pred = A * x_pred;
        P_pred = A * P_pred * A' + Q;
        
        % 更新
        K = P_pred * H' / (H * P_pred * H' + R);
        x_smooth(:,i) = x_pred + K * (y(i) - H * x_pred);
        P_smooth(:,:,i) = (eye(size(K, 1)) - K * H) * P_pred;
        
        % 更新预测值和协方差矩阵
        x_pred = x_smooth(:,i);
        P_pred = P_smooth(:,:,i);
    end
end