用MATLAB写一个卡尔曼滤波算法
时间: 2024-03-09 13:21:25 浏览: 21
以下是一个简单的卡尔曼滤波器的MATLAB代码示例。该滤波器用于估计一个具有常量速度的运动物体的位置和速度。
首先,定义系统的动态模型和观测模型,以及初始状态和协方差矩阵:
```
% 系统的动态模型
F = [1 1; 0 1]; % 状态转移矩阵
G = [0.5 0; 1 0]; % 控制输入矩阵
Q = [0.01 0; 0 0.001]; % 系统噪声协方差矩阵
% 观测模型
H = [1 0; 0 1]; % 观测矩阵
R = [1 0; 0 1]; % 观测噪声协方差矩阵
% 初始状态和协方差矩阵
x0 = [0; 0]; % 初始状态矢量
P0 = [1 0; 0 1]; % 初始状态协方差矩阵
```
然后,生成一组模拟数据作为输入:
```
% 生成模拟数据
N = 100; % 数据长度
u = zeros(2, N); % 控制输入矢量
for k = 1:N-1
u(:,k) = [0.2; 0]; % 假设控制输入为恒定速度
end
x_true = zeros(2, N); % 真实状态矢量
x_true(:,1) = x0;
for k = 2:N
x_true(:,k) = F*x_true(:,k-1) + G*u(:,k-1) + mvnrnd(zeros(1,2), Q)'; % 系统动态模型
end
y = zeros(2, N); % 观测数据矢量
for k = 1:N
y(:,k) = H*x_true(:,k) + mvnrnd(zeros(1,2), R)'; % 观测模型
end
```
接下来,执行卡尔曼滤波器算法:
```
% 卡尔曼滤波器
x_hat = zeros(2, N); % 估计状态矢量
x_hat(:,1) = x0;
P = P0; % 状态协方差矩阵
for k = 2:N
% 预测步骤
x_pred = F*x_hat(:,k-1) + G*u(:,k-1); % 预测状态矢量
P_pred = F*P*F' + Q; % 预测状态协方差矩阵
% 更新步骤
K = P_pred*H'/(H*P_pred*H' + R); % 卡尔曼增益矩阵
x_hat(:,k) = x_pred + K*(y(:,k) - H*x_pred); % 更新状态矢量
P = (eye(2) - K*H)*P_pred; % 更新状态协方差矩阵
end
```
最后,绘制真实状态和估计状态的图像:
```
% 绘图
t = 1:N;
figure;
subplot(2,1,1);
plot(t, x_true(1,:), 'b', t, x_hat(1,:), 'r');
legend('真实位置', '估计位置');
ylabel('位置');
subplot(2,1,2);
plot(t, x_true(2,:), 'b', t, x_hat(2,:), 'r');
legend('真实速度', '估计速度');
xlabel('时间');
ylabel('速度');
```