请编写一个matlab使用的卡尔曼滤波代码
时间: 2024-04-30 17:24:29 浏览: 20
以下是一个简单的 MATLAB 卡尔曼滤波代码实现:
```matlab
% 初始化模型参数
A = [1 1; 0 1]; % 状态转移矩阵
C = [1 0]; % 观测矩阵
Q = [0.1 0; 0 0.1]; % 状态噪声协方差
R = 1; % 观测噪声协方差
% 初始化初始状态和协方差矩阵
x0 = [0; 0]; % 初始状态
P0 = [1 0; 0 1]; % 初始协方差矩阵
% 生成带噪声的状态和观测数据
T = 100; % 数据长度
x = zeros(2,T); % 状态数据
y = zeros(1,T); % 观测数据
for t=1:T
if t==1
x(:,t) = x0;
else
x(:,t) = A*x(:,t-1) + mvnrnd([0;0], Q)';
end
y(:,t) = C*x(:,t) + sqrt(R)*randn;
end
% 卡尔曼滤波
x_hat = zeros(2,T); % 状态估计值
P = zeros(2,2,T); % 协方差矩阵
K = zeros(2,1,T); % 卡尔曼增益
x_hat(:,1) = x0;
P(:,:,1) = P0;
for t=2:T
% 预测
x_hat(:,t) = A*x_hat(:,t-1);
P(:,:,t) = A*P(:,:,t-1)*A' + Q;
% 更新
K(:,:,t) = P(:,:,t)*C'/(C*P(:,:,t)*C' + R);
x_hat(:,t) = x_hat(:,t) + K(:,:,t)*(y(:,t) - C*x_hat(:,t));
P(:,:,t) = (eye(2) - K(:,:,t)*C)*P(:,:,t);
end
% 绘图
figure;
plot(x(1,:), x(2,:), 'b--');
hold on;
plot(x_hat(1,:), x_hat(2,:), 'r-');
xlabel('状态1');
ylabel('状态2');
legend('真实状态', '估计状态');
```
在这个例子中,我们使用了一个简单的线性状态空间模型来演示卡尔曼滤波的实现。我们首先生成了带噪声的状态和观测数据,然后使用卡尔曼滤波算法对状态进行估计。最后,我们绘制了真实状态和估计状态之间的比较图。