卡尔曼滤波过滤matlab代码

以下是一个简单的卡尔曼滤波器的MATLAB代码示例：

%% 卡尔曼滤波器示例代码
clear all;
close all;
clc;

% 定义模型参数
dt = 0.1; % 时间步长
A = [1 dt; 0 1]; % 状态转移矩阵
B = [dt^2/2; dt]; % 输入控制矩阵
H = [1 0]; % 观测矩阵
Q = 0.1*eye(2); % 状态噪声协方差矩阵
R = 1; % 观测噪声协方差矩阵

% 初始化状态
x = [0; 0]; % 初始状态
P = eye(2); % 初始状态协方差矩阵

% 生成模拟数据
t = 0:dt:10; % 时间序列
u = sin(t); % 输入控制信号
y = x(1,:) + 0.1*randn(size(t)); % 观测信号

% 卡尔曼滤波
for i = 1:length(t)
    % 预测
    x_pred = A*x + B*u(i);
    P_pred = A*P*A' + Q;
    
    % 更新
    K = P_pred*H'/(H*P_pred*H' + R);
    x = x_pred + K*(y(i) - H*x_pred);
    P = (eye(2) - K*H)*P_pred;
    
    % 存储结果
    x_est(:,i) = x;
end

% 可视化结果
plot(t,y,'r',t,x_est(1,:),'b');
legend('观测信号','滤波结果');
xlabel('时间');
ylabel('状态值');

这个例子演示了如何使用卡尔曼滤波器对一个简单的线性系统进行状态估计。在代码中，我们首先定义了系统模型的参数，包括状态转移矩阵、观测矩阵、噪声协方差矩阵等。然后我们初始化系统的状态和状态协方差矩阵，并生成了一个模拟的输入控制信号和观测信号。接着，我们使用卡尔曼滤波器对观测信号进行滤波，得到了状态估计结果。最后，我们将观测信号和估计结果可视化。