DR-CAN 卡尔曼滤波笔记

Kalman Filter（卡尔曼滤波）

Optimal(最优化) Recursive(递归) Data Processing(数据处理) Algorithm(算法)

1 递归算法_Recursive Alorithm

1.1 公式推演

当k-1次的估计误差远大于k次的测量误差时， 估计值接近于测量值(估计误差大相信测量值)；

当k-1次的估计误差远小于k次的测量误差时， 估计值接近于上一次估计值；

1.2 案例

1.3编程实现

% 设置迭代次数
n = 5000000;

% 生成测量值序列
% rand(n + 1, 1) 生成一个长度为 n+1 的列向量，元素取值范围是 [0, 1]
% 这里将其线性变换到 [47, 53] 区间，模拟测量值
Z = 47 + 6 * rand(n + 1, 1);

% 测量误差的固定值，代表测量过程中存在的误差
e_mea = 3;

% 初始化估计误差向量，用于存储每次迭代的估计误差
% 初始化为全零向量，长度为 n
e_est = zeros(n, 1);
% 设置估计误差的初始值
e_est(1) = 5;

% 初始化状态估计向量，用于存储每次迭代对状态的估计值
% 初始化为全零向量，长度为 n
X_hat = zeros(n, 1);
% 设置状态估计的初始值
X_hat(1) = 49;

% 卡尔曼滤波迭代过程，从第 2 次迭代开始，到第 n+1 次迭代结束
for k = 2:n + 1
    % 计算卡尔曼增益 K_k
    % 卡尔曼增益用于权衡上一次的估计值和当前测量值在更新状态估计时的权重
    K_k = e_est(k - 1) / (e_est(k - 1) + e_mea);
    
    % 根据卡尔曼增益更新状态估计值
    % 将上一次的估计值与测量值和上一次估计值的差值进行加权求和
    X_hat(k) = X_hat(k - 1) + K_k * (Z(k) - X_hat(k - 1));
    
    % 更新估计误差
    % 根据卡尔曼增益对上次的估计误差进行调整
    e_est(k) = (1 - K_k) * e_est(k - 1);
end

% 计算状态估计值与真实值（这里假设真实值为 50）之间的误差
E = 50 * ones(length(X_hat), 1) - X_hat;

% 创建第一个图形窗口
figure(1);
% 绘制状态估计值随迭代次数的变化曲线
% 横坐标为迭代次数，纵坐标为状态估计值
plot((1:length(X_hat)), X_hat);
% 在图形中显示网格线，方便观察数据
grid on;

% 创建第二个图形窗口
figure(2);
% 绘制估计误差随迭代次数的变化曲线
% 横坐标为迭代次数，纵坐标为估计误差
plot((1:length(X_hat)), E);
% 在图形中显示网格线，方便观察数据
grid on;

 【卡尔曼滤波器】1_递归算法_Recursive Processing_哔哩哔哩_bilibili