卡尔曼滤波器

问题描述

状态空间方程：

定义先验估计

根据测量值的估计结果：

将以上两者融合起来为：

将测量值代入得到：

令

可以得到：

卡尔曼增益求解

引入误差估计：

计算协方差矩阵：

Pk被称为后验状态估计误差的协方差矩阵。它越小，说明估计值越准确。即找到最优的卡尔曼增益K。使得后验状态估计误差的迹最小。即

其中：

后验状态估计协方差矩阵的迹为：

求其最小值，可令：

卡尔曼滤波算法完整流程：

一维卡尔曼滤波示例代码

%% 一维匀速运动卡尔曼滤波示例
clc; clear; close all;

% ========== 参数设置 ==========
dt = 0.1;          % 采样时间(s)
N = 100;           % 时间步数

% 状态转移矩阵 F
F = [1 dt;
     0 1];

% 观测矩阵 H（只能观测位置）
H = [1 0];

% 过程噪声协方差 Q
Q = [0.01  0;
     0    0.01];

% 观测噪声协方差 R
R = 0.5;

% 初始状态 [位置; 速度]
x_hat = [0; 0];       % 初始估计
P = eye(2);           % 初始估计协方差

% ========== 生成真实数据和观测数据 ==========
true_pos = zeros(N,1);
true_vel = ones(N,1) * 1;    % 真实速度为 1 m/s
measurements = zeros(N,1);

true_pos(1) = 0;
for k = 2:N
    true_pos(k) = true_pos(k-1) + true_vel(k)*dt;
end

% 添加观测噪声
rng(42);  % 固定随机种子，方便复现
measurements = true_pos + sqrt(R)*randn(N,1);

% ========== 卡尔曼滤波 ==========
est_pos = zeros(N,1);
est_vel = zeros(N,1);

for k = 1:N
    % ---- 1. 预测 ----
    x_hat_minus = F * x_hat;              % 先验估计
    P_minus = F * P * F' + Q;             % 先验协方差
    
    % ---- 2. 更新 ----
    K = P_minus * H' / (H * P_minus * H' + R);   % 卡尔曼增益
    
    x_hat = x_hat_minus + K * (measurements(k) - H * x_hat_minus);
    P = (eye(2) - K * H) * P_minus;
    
    % 保存结果
    est_pos(k) = x_hat(1);
    est_vel(k) = x_hat(2);
end

% ========== 绘图 ==========
figure;
subplot(2,1,1);
plot(0:dt:(N-1)*dt, true_pos, 'g--', 'LineWidth', 2); hold on;
plot(0:dt:(N-1)*dt, measurements, 'ro', 'MarkerSize', 4); hold on;
plot(0:dt:(N-1)*dt, est_pos, 'b-', 'LineWidth', 2);
legend('真实位置', '观测值', '卡尔曼估计');
xlabel('时间 (s)'); ylabel('位置 (m)');
title('一维卡尔曼滤波 --- 位置估计');
grid on;

subplot(2,1,2);
plot(0:dt:(N-1)*dt, true_vel(1)*ones(N,1), 'g--', 'LineWidth', 2); hold on;
plot(0:dt:(N-1)*dt, est_vel, 'b-', 'LineWidth', 2);
legend('真实速度', '速度估计');
xlabel('时间 (s)'); ylabel('速度 (m/s)');
title('速度估计');
grid on;