用matlab写一个扩展卡尔曼滤波算法
时间: 2024-03-09 11:18:36 浏览: 21
下面是Matlab中扩展卡尔曼滤波(EKF)算法的示例代码:
```
% 初始化
x = [0; 0; 0]; % 状态向量
P = eye(3); % 状态协方差矩阵
Q = eye(3); % 系统噪声协方差矩阵
R = eye(2); % 观测噪声协方差矩阵
dt = 0.1; % 时间间隔
% 系统模型
sys_f = @(x, dt) [x(1) + x(2) * dt; x(2); x(3)];
F = @(x, dt) [1, dt, 0; 0, 1, 0; 0, 0, 1];
% 观测模型
sys_h = @(x) [x(1); x(2)];
H = @(x) [1, 0, 0; 0, 1, 0];
% 生成随机数据进行测试
t = 0:dt:10;
u = randn(1, length(t));
z = [sin(t); cos(t)] + randn(2, length(t));
% 扩展卡尔曼滤波
for i = 1:length(t)
% 预测
x_pred = sys_f(x, dt);
P_pred = F(x, dt) * P * F(x, dt)' + Q;
% 更新
y = z(:, i) - sys_h(x_pred);
S = H(x_pred) * P_pred * H(x_pred)' + R;
K = P_pred * H(x_pred)' * inv(S);
x = x_pred + K * y;
P = (eye(3) - K * H(x_pred)) * P_pred;
end
% 绘制结果
figure;
plot(z(1,:), z(2,:), 'b');
hold on;
plot(x(1), x(2), 'ro');
xlabel('sin(t)');
ylabel('cos(t)');
legend('观测', '滤波结果');
```
在本代码中,我们首先定义了系统模型和观测模型,并初始化了状态向量、状态协方差矩阵、系统噪声协方差矩阵和观测噪声协方差矩阵等参数。我们还生成了随机数据进行测试。
在实际应用中,系统模型和观测模型需要根据具体的问题进行定义。在扩展卡尔曼滤波中,我们需要对系统模型进行线性化处理,而观测模型则可以直接使用。
在循环中,我们首先进行预测,计算出预测状态向量和预测状态协方差矩阵。然后我们使用观测值进行更新,计算出卡尔曼增益、更新后的状态向量和更新后的状态协方差矩阵。
最后,我们将滤波结果与原始观测数据进行比较,绘制出结果。