写卡尔曼滤波的matlab程序
时间: 2023-07-22 19:05:46 浏览: 57
以下是一个简单的卡尔曼滤波的 MATLAB 程序示例:
假设我们有一个由下面的方程给出的线性系统:
x(k+1) = Ax(k) + Bu(k) + w(k)
y(k) = Cx(k) + v(k)
其中,x 是状态向量,A 是状态转移矩阵,B 是输入矩阵,u 是输入向量,w 是过程噪声,y 是观测向量,C 是观测矩阵,v 是观测噪声。
我们要估计状态向量 x(k)。首先,我们需要初始化卡尔曼滤波器的状态向量和协方差矩阵。然后,我们按照以下步骤进行卡尔曼滤波:
1. 预测状态向量和协方差矩阵:
x_hat(k|k-1) = A*x_hat(k-1|k-1) + B*u(k-1)
P(k|k-1) = A*P(k-1|k-1)*A' + Q
其中,x_hat(k|k-1) 是预测的状态向量,P(k|k-1) 是预测的协方差矩阵,Q 是过程噪声的协方差矩阵。
2. 计算卡尔曼增益:
K(k) = P(k|k-1)*C'*(C*P(k|k-1)*C' + R)^(-1)
其中,K(k) 是卡尔曼增益,R 是观测噪声的协方差矩阵。
3. 更新状态向量和协方差矩阵:
x_hat(k|k) = x_hat(k|k-1) + K(k)*(y(k) - C*x_hat(k|k-1))
P(k|k) = (I - K(k)*C)*P(k|k-1)
其中,x_hat(k|k) 是更新后的状态向量,P(k|k) 是更新后的协方差矩阵,I 是单位矩阵。
下面是一个简单的 MATLAB 程序,用于实现卡尔曼滤波器:
```
% 初始化参数
A = [1 1; 0 1];
B = [0.5; 1];
C = [1 0];
Q = [0.01 0; 0 0.1];
R = 1;
x_hat = [0; 0];
P = eye(2);
% 生成数据
u = sin(0:0.1:10)';
y = C*filter(B, [1 A-1], u) + sqrt(R)*randn(length(u), 1);
% 卡尔曼滤波
for k = 1:length(u)
% 预测
x_hat_pred = A*x_hat(:, k) + B*u(k);
P_pred = A*P(:, :, k)*A' + Q;
% 更新
K = P_pred*C'/(C*P_pred*C' + R);
x_hat(:, k+1) = x_hat_pred + K*(y(k) - C*x_hat_pred);
P(:, :, k+1) = (eye(2) - K*C)*P_pred;
end
% 绘制结果
figure;
subplot(211);
plot(u);
title('Input');
subplot(212);
plot(y, 'r');
hold on;
plot(C*x_hat(:, 2:end), 'b');
title('Output');
legend('Measured', 'Estimated');
```
这段代码首先初始化了系统参数和状态向量,然后生成了一些模拟数据。在主循环中,它按照上述步骤进行卡尔曼滤波,并更新状态向量和协方差矩阵。最后,它绘制了输入和输出信号,并将估计结果与测量结果进行比较。