扩展卡尔曼滤波算法matlab

扩展卡尔曼滤波算法（EKF）是一种常用的非线性系统状态估计方法，它通过将非线性系统在每个时刻进行线性化，然后使用标准的卡尔曼滤波算法来进行状态估计。在实际应用中，EKF常用于机器人导航、目标跟踪、信号处理等领域。

以下是一份基于Matlab的EKF代码示例：

% EKF algorithm demo

% define system and measurement models
f = @(x) [x(1)+0.1*x(2);0.8*x(2)-0.2*x(1)]; % state transition function
h = @(x) [x(1)^2+0.5*x(2)^2]; % measurement function

% generate noise
Q = diag([0.01,0.01]); % state noise covariance matrix
R = 1; % measurement noise covariance

% initial state and covariance
x0 = [0;0]; % initial state
P0 = diag([1,1]); % initial covariance

% simulate system
N = 100; % number of time steps
x_true = zeros(2,N); % true state
x_true(:,1) = x0;
y = zeros(1,N); % measurements
for k = 2:N
    x_true(:,k) = f(x_true(:,k-1)) + mvnrnd([0;0],Q)'; % propagate true state
    y(k) = h(x_true(:,k)) + sqrt(R)*randn; % generate measurement
end

% EKF algorithm
x_est = zeros(2,N); % estimated state
x_est(:,1) = x0; % initial estimate
P = P0; % initial covariance estimate
for k = 2:N
    % propagate estimate
    x_pred = f(x_est(:,k-1));
    F = jacobian(f,x_est(:,k-1));
    P_pred = F*P*F' + Q;

    % update estimate
    H = jacobian(h,x_pred);
    K = P_pred*H'/(H*P_pred*H' + R);
    x_est(:,k) = x_pred + K*(y(k) - h(x_pred));
    P = (eye(2) - K*H)*P_pred;
end

% plot results
figure;
subplot(2,1,1);
plot(1:N,x_true(1,:),1:N,x_est(1,:),'--');
xlabel('Time');
ylabel('Position');
legend('True state','Estimated state');
subplot(2,1,2);
plot(1:N,x_true(2,:),1:N,x_est(2,:),'--');
xlabel('Time');
ylabel('Velocity');
legend('True state','Estimated state');

% jacobian function
function J = jacobian(f,x)
    h = 1e-6;
    n = length(x);
    fx = f(x);
    J = zeros(length(fx),n);
    for i = 1:n
        x1 = x;
        x1(i) = x1(i) + h;
        fx1 = f(x1);
        J(:,i) = (fx1 - fx)/h;
    end
end

在以上示例中，我们定义了一个非线性系统模型和一个非线性测量模型，然后生成了一些噪声并模拟了系统状态和测量值。接着，我们使用EKF算法对系统状态进行估计，并将结果与真实状态进行比较。最后，我们使用一个jacobian函数来计算状态转移函数和测量函数的雅可比矩阵，以便进行线性化。

需要注意的是，在实际应用中，EKF算法可能存在一些限制和缺陷，例如对初始状态和噪声统计信息的要求较高，对非线性程度的限制等。因此，在使用EKF算法时需要根据具体应用场景进行合理选择和调整。