平台：matlab2021b，Vivado2018
目录
应用场景
在实际的应用中，模拟信号在经过adc采集后转换为数字信号，但是这个转换的值往往会有很大噪声，这里我们使用卡尔曼滤波器对采集的数据进行滤波处理。例如，我们使用信号源输出一个1v的直流电，经过adc转换后测试得到一组数据，t0时刻0.995v，0.999v，1.005v。因为我们输出信号为1v始终没有改变，所以我们预测现在的电压依旧是1v。但是真实的电压是多少呢，是我们相信信号源输出的1v，还是相信我们adc采集的电压0.995v呢？这时候就可以使用卡尔曼滤波器来对采集的数据进行滤波处理。
卡尔曼滤波器的五大核心公式
预测部分
状态更新方程
以上就是卡尔曼滤波器的五大核心公式。
对于这个公式
参数解释
表示先验预测值，并不准确。
上一个时刻的状态。
是系统控制量，我们这里没有系统控制增益，即为0。
A，B都是系统参数，都是矩阵，A用来表示值变化的一个过程，我们这里输入是恒定不变的，所以A取1。因为系统不存在控制量，所以B取0。
所以这里公式可以化简为
代表我们当前时刻的预测值和前一时刻的值是不变的。
预测误差的协方差矩阵，表示k时刻系统的不确定性。代表前一时刻值和当前时刻值的可信度。由于卡尔曼滤波的过程是不断更新该值的过程，这里将这个值初值设置为1（第一次完全可信）
那么这个Q是什意思呢？过程噪声的协方差矩阵。这个值的大小决定你是相信你的预测值还是相信实际测量值。越大表示越相信测量值。所以这里我们随便设置一个值Q为1。
所以公式变为
下面看一下更新方程。
表示为卡尔曼增益，这个值也是一个不断跟新的过程。表示我们在计算的过程中是相信预测值还是相信测量值。如果相信预测值，则该值变小，如果相信测量值，则该值变大。
H表示观测矩阵。此处过于复杂，在我们这个应用中另H为1。具体的求取过程可以参考
无人驾驶技术入门（十八）| 手把手教你写扩展卡尔曼滤波器 - 知乎 (zhihu.com)
R表示观测噪声的协方差矩阵，反应的是测量值与真实值之间的不确定性，这里就是我们adc采集的误差。R越大，噪声越大，测量值越不靠谱，则卡尔曼增益K_k越趋近于0，越相信预测值。R越小，测量值越准确。越相信测量值。
所以上述状态更新方程公式化简为
就是我们最后得到估计值。就是经过滤波后计算出的值。
就是测量值。
所以卡尔曼滤波的计算过程可以简化为
求先验预测值，求预测误差的协方差矩阵，求卡尔曼增益，求滤波后的值，在更新预测误差的协方差矩阵。
matlab实现
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)