卡尔曼滤波器（Kalman-Filter）算法Matlab程序

function [x_estimate, P_estimate] = KalmanFilter(z, x_init, P_init, A, H, Q, R)
% Perform Kalman filtering on a given time series of measurements.
% Inputs:
%   z: measurement data
%   x_init: initial state estimate
%   P_init: initial covariance estimate
%   A: state transition matrix
%   H: observation matrix
%   Q: process noise covariance
%   R: observation noise covariance
% Outputs:
%   x_estimate: estimated state trajectory
%   P_estimate: estimated covariance trajectory

x_estimate = x_init;
P_estimate = P_init;

for i = 1:length(z)
    % Time update (prediction)
    x_predict = A * x_estimate;
    P_predict = A * P_estimate * A' + Q;
    
    % Measurement update (correction)
    K = P_predict * H' / (H * P_predict * H' + R);
    x_estimate = x_predict + K * (z(i) - H * x_predict);
    P_estimate = (eye(size(A)) - K * H) * P_predict;
    
    % Store the results
    x_estimate_trajectory(:,i) = x_estimate;
    P_estimate_trajectory(:,:,i) = P_estimate;
end