编写一个卡尔曼滤波的matlab程序
时间: 2023-08-04 14:39:27 浏览: 42
好的,以下是一个简单的卡尔曼滤波的 MATLAB 程序示例:
```matlab
% 定义系统模型
A = [1 1; 0 1]; % 状态转移矩阵
B = [0.5; 1]; % 输入矩阵
C = [1 0]; % 测量矩阵
Q = [1e-4 0; 0 1e-2]; % 系统噪声协方差
R = 1; % 测量噪声协方差
% 初始状态估计和协方差矩阵
x_hat = [0; 0];
P = [1 0; 0 1];
% 生成输入和测量数据
N = 100;
u = zeros(1, N);
y = zeros(1, N);
for k = 1:N
u(k) = sin(k/10);
y(k) = C*x_hat + sqrt(R)*randn; % 添加高斯噪声
x_hat_minus = A*x_hat + B*u(k); % 先验状态估计
P_minus = A*P*A' + Q; % 先验协方差估计
K = P_minus*C'/(C*P_minus*C' + R); % 卡尔曼增益
x_hat = x_hat_minus + K*(y(k) - C*x_hat_minus); % 后验状态估计
P = (eye(2) - K*C)*P_minus; % 后验协方差估计
end
% 绘制结果
t = 1:N;
figure;
plot(t, u, 'r', t, y, 'g', t, x_hat(1,:), 'b');
legend('输入信号', '测量信号', '状态估计');
```
此程序使用一个简单的线性系统模型,并假设系统噪声和测量噪声都是高斯分布。程序输出输入信号、测量信号和估计状态的图形。请注意,这只是一个简单的示例程序,实际应用中需要根据具体情况进行修改和优化。