卡尔曼滤波器（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