扩展卡尔曼滤波器matlab

在 MATLAB 中实现扩展卡尔曼滤波器，您可以按照以下步骤进行操作：

1. 定义系统模型：首先，您需要定义您的系统模型，包括状态转移矩阵、观测矩阵、过程噪声协方差矩阵和观测噪声协方差矩阵。

2. 初始化滤波器：定义初始状态估计和初始协方差矩阵。

3. 预测步骤：使用系统模型进行状态预测，同时更新协方差矩阵。

4. 更新步骤：使用观测值进行状态修正，同时更新协方差矩阵。

下面是一个简单的示例代码，演示如何在 MATLAB 中实现扩展卡尔曼滤波器：

% 定义系统模型
A = [1 1; 0 1];  % 状态转移矩阵
H = [1 0];      % 观测矩阵
Q = [0.01 0; 0 0.01];   % 过程噪声协方差矩阵
R = 1;         % 观测噪声协方差矩阵

% 初始化滤波器
x = [0; 0];    % 初始状态估计
P = [1 0; 0 1];   % 初始协方差矩阵

% 模拟观测数据
T = 100;       % 时间步数
true_x = zeros(2, T);   % 真实状态
obs = zeros(1, T);      % 观测值
for t = 1:T
    true_x(:, t+1) = A * true_x(:, t) + mvnrnd([0; 0], Q)';
    obs(:, t) = H * true_x(:, t+1) + sqrt(R) * randn;
end

% 扩展卡尔曼滤波器
filtered_x = zeros(2, T);   % 滤波后的状态估计
for t = 1:T
    % 预测步骤
    x_pred = A * x;
    P_pred = A * P * A' + Q;
    
    % 更新步骤
    y = obs(:, t) - H * x_pred;
    S = H * P_pred * H' + R;
    K = P_pred * H' / S;
    x = x_pred + K * y;
    P = (eye(2) - K * H) * P_pred;
    
    filtered_x(:, t) = x;   % 存储滤波后的状态估计
end

% 绘制结果
figure;
plot(1:T, true_x(1, 2:end), 'b-', 'LineWidth', 2);
hold on;
plot(1:T, obs, 'r.', 'MarkerSize', 10);
plot(1:T, filtered_x(1, :), 'g-', 'LineWidth', 2);
legend('真实状态', '观测值', '滤波状态');
xlabel('时间步数');
ylabel('状态值');

请注意，这只是一个简单的示例，您可以根据自己的需求进行修改和扩展。希望对您有所帮助！