卡尔曼滤波算法matlab代码实现
时间: 2023-08-04 19:00:29 浏览: 58
卡尔曼滤波算法是一种用于线性动态系统状态估计的优秀算法。下面是一个用MATLAB实现卡尔曼滤波算法的代码示例:
```matlab
% 卡尔曼滤波算法MATLAB代码实现示例
% 初始化变量
dt = 0.1; % 采样时间间隔
N = 100; % 数据点个数
t = (0:N-1)*dt; % 时间序列
x_true = sin(t); % 真实状态值
% 生成带有噪声的观测值
R = 0.1; % 观测噪声方差
z = x_true + sqrt(R)*randn(size(t)); % 观测序列
% 定义状态转移矩阵
A = 1; % 状态转移矩阵
B = 0; % 控制输入矩阵
H = 1; % 观测矩阵
% 定义初始状态估计和协方差矩阵
x_est = 0; % 初始状态估计值
P_est = 1; % 初始状态估计的协方差矩阵
% 定义过程噪声和测量噪声协方差矩阵
Q = 0.01; % 过程噪声方差
R = 0.1; % 观测噪声方差
% 存储卡尔曼滤波估计值
x_kf = zeros(size(t));
P_kf = zeros(size(t));
% 运行卡尔曼滤波算法
for k = 1:N
% 预测步骤
x_pred = A*x_est;
P_pred = A*P_est*A' + Q;
% 更新步骤
K = P_pred*H'/(H*P_pred*H' + R);
x_est = x_pred + K*(z(k) - H*x_pred);
P_est = (eye(size(K*H)) - K*H)*P_pred;
% 存储估计结果
x_kf(k) = x_est;
P_kf(k) = P_est;
end
% 绘制结果图形
figure;
plot(t,x_true,'b-',t,z,'r.','MarkerSize',8,'LineWidth',1.5);
hold on;
plot(t,x_kf,'m--','LineWidth',1.5);
legend('真实状态','观测值','卡尔曼滤波估计');
xlabel('时间');
ylabel('状态值');
title('卡尔曼滤波算法结果');
```
这段代码实现了一个简单的一维卡尔曼滤波算法,其中预测步骤根据状态转移矩阵A和过程噪声Q预测下一时刻的状态和协方差;更新步骤根据观测矩阵H、观测噪声R和观测值z对预测结果进行调整。最终,通过循环迭代对整个时间序列进行滤波估计,并绘制出真实状态、观测值和卡尔曼滤波估计结果的图形。