扩展卡尔曼滤波 matlab代码
时间: 2023-07-22 18:59:25 浏览: 75
以下一个简单的扩展卡尔曼滤波(E)的Matlab示例代码:
```matlab% 初始化
x = [0; 0]; % 初始状态向量 [位置; 速度]
P = eye(2); % 初始状态协方差矩阵
dt = 0.1; % 时间间隔
% 系统模型
A = [1 dt; 0 1]; % 状态转移矩阵
B = [0.5*dt^2; dt]; % 输入矩阵
H = [1 0]; % 观测矩阵
Q = diag([0.1, 0.1]); % 系统噪声协方差矩阵
R = 1; % 观测噪声方差
% 数据生成
T = 10; % 总时间
N = T/dt; % 数据点个数
u = sin(0:dt:T-dt)'; % 控制输入
x_true = zeros(N, 2); % 实际状态
z = zeros(N, 1); % 观测值
for k = 1:N
x_true(k, :) = A*x + B*u(k) + sqrt(Q)*randn(2, 1); % 状态更新
z(k) = H*x_true(k, :) + sqrt(R)*randn; % 观测更新
x = A*x + B*u(k) + sqrt(Q)*randn(2, 1); % 状态预测
end
% 扩展卡尔曼滤波
x_est = zeros(N, 2); % 估计状态
P_est = zeros(2, 2, N); % 估计状态协方差矩阵
x_est(1, :) = [0; 0]; % 初始估计状态
P_est(:, :, 1) = eye(2); % 初始估计状态协方差矩阵
for k = 2:N
% 预测步骤
x_pred = A*x_est(k-1, :) + B*u(k);
P_pred = A*P_est(:, :, k-1)*A' + Q;
% 更新步骤
K = P_pred*H'/(H*P_pred*H' + R);
x_est(k, :) = x_pred' + K*(z(k) - H*x_pred);
P_est(:, :, k) = (eye(2) - K*H)*P_pred;
end
% 可视化结果
t = 0:dt:T-dt;
figure;
subplot(2, 1, 1);
plot(t, x_true(:, 1), 'b', t, x_est(:, 1), 'r');
legend('真实位置', '估计位置');
xlabel('时间');
ylabel('位置');
subplot(2, 1, 2);
plot(t, x_true(:, 2), 'b', t, x_est(:, 2), 'r');
legend('真实速度', '估计速度');
xlabel('时间');
ylabel('速度');
```
这段代码实现了一个简单的一维运动模型,其中使用了扩展卡尔曼滤波来估计位置和速度。代码中的注释会帮助你理解每一步的操作。你可以根据需要进行修改和扩展。希望对你有帮助!
相关推荐
![m](https://img-home.csdnimg.cn/images/20210720083646.png)
![](https://csdnimg.cn/download_wenku/file_type_ask_c1.png)
![](https://csdnimg.cn/download_wenku/file_type_ask_c1.png)
![](https://csdnimg.cn/download_wenku/file_type_ask_c1.png)
![](https://csdnimg.cn/download_wenku/file_type_ask_c1.png)
![](https://csdnimg.cn/download_wenku/file_type_ask_c1.png)
![](https://csdnimg.cn/download_wenku/file_type_ask_c1.png)
![](https://csdnimg.cn/download_wenku/file_type_ask_c1.png)
![](https://csdnimg.cn/download_wenku/file_type_ask_c1.png)
![](https://csdnimg.cn/download_wenku/file_type_ask_c1.png)
![](https://csdnimg.cn/download_wenku/file_type_ask_c1.png)
![](https://csdnimg.cn/download_wenku/file_type_ask_c1.png)
![](https://csdnimg.cn/download_wenku/file_type_ask_c1.png)
![](https://csdnimg.cn/download_wenku/file_type_ask_c1.png)
![](https://csdnimg.cn/download_wenku/file_type_ask_c1.png)
![](https://csdnimg.cn/download_wenku/file_type_ask_c1.png)