卡尔曼滤波matlab代码
时间: 2023-09-23 14:07:22 浏览: 41
以下是一个简单的卡尔曼滤波MATLAB代码示例:
```matlab
% 初始化参数
dt = 0.1; % 时间间隔
A = [1 dt; 0 1]; % 状态转移矩阵
C = [1 0]; % 观测矩阵
Q = [0.1 0; 0 0.1]; % 状态噪声协方差矩阵
R = 1; % 观测噪声协方差
% 初始化初始状态及协方差矩阵
x0 = [0; 0];
P0 = [1 0; 0 1];
% 生成模拟数据
t = 0:dt:10;
n = length(t);
x = zeros(2, n);
y = zeros(1, n);
x(:, 1) = [0; 0];
for i = 2:n
x(:, i) = A * x(:, i-1) + sqrt(Q) * randn(2, 1);
y(i) = C * x(:, i) + sqrt(R) * randn;
end
% 卡尔曼滤波
xhat = zeros(2, n);
xhat(:, 1) = x0;
Phat = zeros(2, 2, n);
Phat(:, :, 1) = P0;
for i = 2:n
% 预测
xhat(:, i) = A * xhat(:, i-1);
Phat(:, :, i) = A * Phat(:, :, i-1) * A' + Q;
% 更新
K = Phat(:, :, i) * C' / (C * Phat(:, :, i) * C' + R);
xhat(:, i) = xhat(:, i) + K * (y(i) - C * xhat(:, i));
Phat(:, :, i) = (eye(2) - K * C) * Phat(:, :, i);
end
% 绘制结果
figure;
plot(t, x(1, :), 'k-', t, y, 'r.', t, xhat(1, :), 'b-');
legend('真实值', '观测值', '卡尔曼滤波估计值');
xlabel('时间');
ylabel('状态1');
```
这个示例代码使用了一个简单的一维状态模型,并且使用了随机噪声来模拟实际情况下的测量误差和控制误差。
相关推荐
![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)
![m](https://img-home.csdnimg.cn/images/20210720083646.png)