扩展卡尔曼滤波（EFK）算法的Matlab程序

function [x_est, P_est] = ExtendedKalmanFilter(x_prev, P_prev, u, z, Q, R)
% ExtendedKalmanFilter - implements the extended Kalman filter algorithm
%
% Inputs:
%    x_prev - the previous state estimate
%    P_prev - the previous state covariance estimate
%    u - the control input
%    z - the measurement
%    Q - the process noise covariance
%    R - the measurement noise covariance
%
% Outputs:
%    x_est - the updated state estimate
%    P_est - the updated state covariance estimate
%
% Example:
%    [x_est, P_est] = ExtendedKalmanFilter(x_prev, P_prev, u, z, Q, R);

% Prediction step
x_pred = f(x_prev, u); % State prediction
F = df_dx(x_prev, u); % Jacobian of the state transition model
P_pred = F * P_prev * F' + Q; % Covariance prediction

% Update step
H = dh_dx(x_pred); % Jacobian of the measurement model
K = P_pred * H' / (H * P_pred * H' + R); % Kalman gain
x_est = x_pred + K * (z - h(x_pred)); % State update
P_est = (eye(size(x_est)) - K * H) * P_pred; % Covariance update

end

function x_pred = f(x, u)
% State transition model
% Example:
%    x_pred = f(x, u);

x_pred = x + u;

end

function F = df_dx(x, u)
% Jacobian of the state transition model
% Example:
%    F = df_dx(x, u);

F = eye(size(x));

end

function z_pred = h(x)
% Measurement model
% Example:
%    z_pred = h(x);

z_pred = x;

end

function H = dh_dx(x)
% Jacobian of the measurement model
% Example:
%    H = dh_dx(x);

H = eye(size(x));

end