卡尔曼滤波器算法的学习与简单使用

爱漂流的易子

于 2024-02-27 20:43:41 发布

阅读量743

点赞数 32

分类专栏： matlab软件与算法实现文章标签：算法 matlab

本文链接：https://blog.csdn.net/hy_520520/article/details/136305294

版权

matlab软件与算法实现专栏收录该内容

5 篇文章 0 订阅

订阅专栏

平台：matlab2021b，Vivado2018

应用场景

卡尔曼滤波器的五大核心公式

matlab实现

应用场景

在实际的应用中，模拟信号在经过adc采集后转换为数字信号，但是这个转换的值往往会有很大噪声，这里我们使用卡尔曼滤波器对采集的数据进行滤波处理。例如，我们使用信号源输出一个1v的直流电，经过adc转换后测试得到一组数据，t0时刻0.995v，0.999v，1.005v。因为我们输出信号为1v始终没有改变，所以我们预测现在的电压依旧是1v。但是真实的电压是多少呢，是我们相信信号源输出的1v，还是相信我们adc采集的电压0.995v呢？这时候就可以使用卡尔曼滤波器来对采集的数据进行滤波处理。

卡尔曼滤波器的五大核心公式

预测部分

$\hat{x}_{k}^{-}=A\hat{x}_{k-1}+Bu_{k-1}$

$P_{k}^{-}=AP_{k-1}A^{T}+Q$

状态更新方程

$K_{k}=\frac{P_{k}^{-}H^{T}}{HP_{k}^{-}H^{T}+R}$

$\hat{x_{k}}=\hat{x}_{k}^{-}+K\left ( z_{k} -H\hat{x}_{k}^{-}\right )$

$P_{k}=\left ( I-K_{k} H\right )P_{k}^{-}$

以上就是卡尔曼滤波器的五大核心公式。

对于这个公式
参数解释
$\hat{x}_{k}^{-}$ 表示先验预测值，并不准确。
$\hat{x}_{k-1}$ 上一个时刻的状态。
$u_{k-1}$ 是系统控制量，我们这里没有系统控制增益，即为0。
A，B都是系统参数，都是矩阵，A用来表示值变化的一个过程，我们这里输入是恒定不变的，所以A取1。因为系统不存在控制量，所以B取0。
所以这里公式可以化简为
$\hat{x}_{k}^{-}=\hat{x}_{k-1}$
代表我们当前时刻的预测值和前一时刻的值是不变的。
$P_{k}^{-}$ 预测误差的协方差矩阵，表示k时刻系统的不确定性。代表前一时刻值和当前时刻值的可信度。由于卡尔曼滤波的过程是不断更新该值的过程，这里将这个值初值设置为1（第一次完全可信）
那么这个Q是什意思呢？过程噪声的协方差矩阵。这个值的大小决定你是相信你的预测值还是相信实际测量值。越大表示越相信测量值。所以这里我们随便设置一个值Q为1。
所以公式变为
$P_{k}^{-}=P_{k-1}+Q$
下面看一下更新方程。
$K_{k}$ 表示为卡尔曼增益，这个值也是一个不断跟新的过程。表示我们在计算的过程中是相信预测值还是相信测量值。如果相信预测值，则该值变小，如果相信测量值，则该值变大。
H表示观测矩阵。此处过于复杂，在我们这个应用中另H为1。具体的求取过程可以参考

无人驾驶技术入门（十八）| 手把手教你写扩展卡尔曼滤波器 - 知乎 (zhihu.com)
R表示观测噪声的协方差矩阵，反应的是测量值与真实值之间的不确定性，这里就是我们adc采集的误差。R越大，噪声越大，测量值越不靠谱，则卡尔曼增益K_k越趋近于0，越相信预测值。R越小，测量值越准确。越相信测量值。
所以上述状态更新方程公式化简为

$K_{k}=\frac{P_{k}^{-}}{P_{k}^{-}+R}$

$\hat{x_{k}}=\hat{x}_{k}^{-}+K\left ( z_{k} -\hat{x}_{k}^{-}\right )$

$P_{k}=\left ( I-K_{k} \right )P_{k}^{-}$

$\hat{x}_{k}^{-}$ 就是我们最后得到估计值。就是经过滤波后计算出的值。
$z_{k}$ 就是测量值。
所以卡尔曼滤波的计算过程可以简化为
求先验预测值，求预测误差的协方差矩阵，求卡尔曼增益，求滤波后的值，在更新预测误差的协方差矩阵。

matlab实现

clc;
clear ;
% 生成随机噪声
numPoints = 4000;  % 数据点数量
noiseAmplitude = 1; %假设测量值是1v加上高斯噪声N(0,1) 
filteredData = zeros(1,numPoints);
noise = noiseAmplitude + 0.01*randn(1, numPoints);%假设采集波动在0.01v左右

% 调用 KalmanFilter 函数进行滤波
for t=1:numPoints
    filteredData(t)= KalmanFilter(noise(t));
end
% 绘制原始数据和滤波后数据的图像
figure;
% 原始数据
plot(1:numPoints, noise, 'b', 'LineWidth', 1.5);  
hold on;
% 滤波后数据
plot(1:numPoints, filteredData, 'r', 'LineWidth', 1.5);  
legend('原始数据', '卡尔曼滤波后数据');
xlabel('数据点');
ylabel('数据值');
title('原始数据和滤波后数据');


% 创建卡尔曼滤波代码
function filteredData = KalmanFilter(inData)
    % 定义持久变量
    persistent prevData %预计值
    persistent p        %预计误差协方差
    persistent q        %过程噪声
    persistent r        %测量噪声协方差
    persistent kGain    %卡尔曼增益

    % 如果 prevData 变量为空，则初始化持久变量的初始值
    if isempty(prevData)
        prevData = 0;   % 先前数据的初始值
        p = 1;          % 先验协方差
        q = 1;          % 过程噪声的方差
        r = 1;          % 测量噪声的方差
        kGain = 0;      % 卡尔曼增益的初始值
    end
    % 更新先验协方差
    p = p + q;
    % 计算卡尔曼增益
    kGain = p / (p + r);
    % 使用卡尔曼增益对观测值进行滤波
    filteredData = prevData + (kGain * (inData - prevData));
    % 更新先验协方差
    p = (1 - kGain) * p;
    % 更新先前数据的值
    prevData = filteredData;
end

这样经过滤波后的波形数据如下。

由前面可以知道，我们Q值的大小决定你是相信你的预测值还是相信实际测量值。这里我们减少Q值为0.01。相信预测值。

可以看到滤波后的数据噪声明显变小了。

R的大小反应的是测量值与真实值之间的不确定性。我们初始设置的为1。这里将R的大小缩小到0.01。

可以看到滤波后的值基本和测量值重合了。

这里将R的大小缩小到10。

参考博文

''说人话"系列之卡尔曼滤波 - 知乎 (zhihu.com)

无人驾驶技术入门（十八）| 手把手教你写扩展卡尔曼滤波器 - 知乎 (zhihu.com)

定位算法——Kalman滤波简要入门(理论+实例+matlab实现)_卡尔曼滤波matlab-CSDN博客

爱漂流的易子

关注

32
点赞
踩
15

收藏

觉得还不错? 一键收藏
0
评论
卡尔曼滤波器算法的学习与简单使用

在实际的应用中，模拟信号在经过adc采集后转换为数字信号，但是这个转换的值往往会有很大噪声，这里我们使用卡尔曼滤波器对采集的数据进行滤波处理。例如，我们使用信号源输出一个1v的直流电，经过adc转换后测试得到一组数据，t0时刻0.995v，0.999v，1.005v。因为我们输出信号为1v始终没有改变，所以我们预测现在的电压依旧是1v。但是真实的电压是多少呢，是我们相信信号源输出的1v，还是相信我们adc采集的电压0.995v呢？这时候就可以使用卡尔曼滤波器来对采集的数据进行滤波处理。
复制链接

扫一扫