卡尔曼滤波matlab代码

以下是一个简单的卡尔曼滤波的 MATLAB 代码实现。这里以一个一维的运动状态为例子，包含位置和速度两个状态量。

% 定义系统状态转移矩阵 A 和观测矩阵 H
dt = 0.1; % 时间间隔
A = [1 dt; 0 1]; % 状态转移矩阵，假设匀加速直线运动
H = [1 0]; % 观测矩阵，只能观测到位置

% 定义系统噪声和观测噪声的协方差矩阵
Q = [0.01 0; 0 0.01]; % 系统噪声协方差矩阵，假设位置和速度的噪声方差相等
R = 0.1; % 观测噪声方差

% 初始化状态向量和协方差矩阵
x = [0; 0]; % 初始状态，假设位置和速度都为零
P = [1 0; 0 1]; % 初始协方差矩阵

% 定义观测数据
t = 0:dt:10; % 时间
y = sin(t); % 观测数据，假设真实运动状态是 sin 函数

% 卡尔曼滤波
x_filtered = zeros(2,length(t)); % 滤波后的状态向量，包含位置和速度
for i = 1:length(t)
    % 预测步骤
    x_pred = A * x; % 预测状态
    P_pred = A * P * A.' + Q; % 预测协方差矩阵

    % 更新步骤
    K = P_pred * H.' * inv(H * P_pred * H.' + R); % 计算卡尔曼增益
    x = x_pred + K * (y(i) - H * x_pred); % 更新状态
    P = (eye(2) - K * H) * P_pred; % 更新协方差矩阵

    % 保存滤波后的状态向量
    x_filtered(:,i) = x;
end

% 绘制结果
figure;
plot(t,y,'r',t,x_filtered(1,:),'b');
legend('真实值','滤波值');
xlabel('时间');
ylabel('位置');

该代码中，首先定义了系统状态转移矩阵 `A` 和观测矩阵 `H`，以及系统噪声和观测噪声的协方差矩阵 `Q` 和 `R`。然后初始化状态向量和协方差矩阵，定义观测数据。接着，使用 for 循环对每个时间步骤进行卡尔曼滤波，包括预测步骤和更新步骤，并保存滤波后的状态向量。最后，绘制真实值和滤波值的对比图。