1.基本概念
卡尔曼滤波（Kalman filter）是一种高效率的递归滤波器（自回归滤波器），它能够从一系列的不完全及包含噪声的测量中，估计动态系统的状态。卡尔曼滤波会根据各测量量在不同时间下的值，考虑各时间下的联合分布，再产生对未知变数的估计，因此会比只以单一测量量为基础的估计方式要准。
简单讲，卡尔曼滤波器既考虑测量值也考虑预测值（根据系统前一次的状态），来决定输出，因此相比传统的滤波器更加准确和可靠。
2.适用范围
卡尔曼滤波适用于线性高斯系统，即该系统必须是线性的，而且噪声服从正态分布（高斯分布）。更详细一些，噪声通常被建模为一个均值为0，方差为常数的正态分布，也就是高斯分布。该正态分布的横坐标是随机变量的取值，纵坐标是对应取值的概率密度。
3.建模
这里的建模是为系统建模，通过建模我们可以清楚的了解，系统是否可以通过卡尔曼滤波器进行处理，模型如下：
其中，
1)xk为k时刻信号值，xk-1为k-1时刻（前一次）信号值
2)uk为控制量，简单应用场景下一般为0
3)A,B,H是与系统有关的矩阵，一般为常数
4)wk为符合高斯分布的过程噪声，其协方差为Q
5)vk为符合高斯分布的测量噪声，其协方差为R
6)zk为测量值
公式1告诉我们信号值xk是前一次信号值xk-1的线性组合加上控制信号及过程噪声，其过程噪声要求符合高斯分布。
公式2告诉我们测量值是信号值的线性组合加上测量噪声，其测量噪声要求符合高斯分布。
通过对系统建模，我们了解系统是否可以通过卡尔曼滤波器进行处理。
4.公式
本文不介绍相关公式的推导，仅列出推导出的结论。想了解其推导过程可参阅其他资料。
其中，
Time Update(prediction)公式：
1) 为根据k-1时刻的值预测k时刻的结果，也写作Xk|k-1
2)为k-1时刻最优值，也写作Xk-1|k-1
3)A为状态下的变换矩阵
4)B为作用在控制量上的变换矩阵
5)uk为控制量，一般情况下为0
6)为根据k-1时刻的值预测k时刻系统协方差矩阵，也写作Pk|k-1
7)为k-1时刻系统协方差矩阵，，也写作Pk-1|k-1
8)Q为系统过程噪声的协方差
Measurement Update(correction)公式：
1)Kk为卡尔曼增益
2)H为对象的预测矩阵，当系统是一维的时候，H=1
3)R为对象测量噪声的协方差矩阵
4)zk为测量值，也就是输入值
5)为k时刻最优值，也就是输出值
A，B，H大多数情况下，它们为常数，系统为一维情况下为1，对于一个卡尔曼滤波器我们需要确定以下参数（难点）：
1)R，初始值可根据传感器测量误差进行选择
2)Q，初始值可根据实际调试确定
3)x0，初始值一般可设置为0
4)P0，初始值一般可设置为1，P0=0表示完全相信测量值，P0=1表示完全相信估计值，一般是不能设置为0，否则Pk就一直为0了，可设置为1，在后续迭代的过程中，Pk会不断被修正
参数调节的一般方法：
根据公式，我们可以分析得出：R越大，Kk越小；Q越大，Kk越大。Kk越大我们越相信测量值，当我们使用的传感器比较精密，测量噪声很小时，我们越相信测量值，此时应该调小R，反之则调大，另外，当我们的系统模型越准确时，我们的预测值会越精确，此时我们越信任预测值，则应该调小Q。
5.迭代
与传统的数字滤波器（FIR，IIR）滤波器一样，卡尔曼滤波器也是采用迭代的方法进行计算。其迭代过程如下图所示。
迭代过程：
1)Time Update(prediction)。根据前一次状态预估下一时刻状态（输出值，错误协方差）
2)Measurement Update(correction)。计算卡尔曼增益，根据测量值（zk）及预估状态，计算当前最优估计值（输出值）
3)将当前状态赋予下一次状态（输出值，错误协方差），便于下一次计算
6.代码
一维的情况下各矩阵（A，H，I）为1，uk为0，要简单许多，直接套用公式即可，这里为了方便描述直接用float数据，如果MCU计算能力有限，也可以用整型数据进行计算，C代码如下：
typedef struct _KALMAN_FILTER
{
float xk_1; //x(k-1|k-1)
float Pk_1; //P(k-1|k-1)
float xkk_1; //x(k|k-1)
float Pkk_1; //P(k|k-1)
float Kk; //Kk
float xk; //x(k|k)
float Pk; //P(k|k)
float Q;
float R;
}KALMAN_FILTER;
static void InitKalmanFilter(KALMAN_FILTER *pKalmanFilter);
static float CalcKalmanFilter(KALMAN_FILTER *pKalmanFilter, float zk);
static void InitKalmanFilter(KALMAN_FILTER *pKalmanFilter)
{
pKalmanFilter->xk_1 = 0;
pKalmanFilter->Pk_1 = 1;
pKalmanFilter->xkk_1 = 0;
pKalmanFilter->Pkk_1 = 0;
pKalmanFilter->Kk = 0;
pKalmanFilter->xk = 0;
pKalmanFilter->Pk = 0;
pKalmanFilter->Q = 0;
pKalmanFilter->R = 0.1f;
}
static float CalcKalmanFilter(KALMAN_FILTER *pKalmanFilter, float zk)
{
//prediction
pKalmanFilter->xkk_1 = pKalmanFilter->xk_1;
pKalmanFilter->Pkk_1 = pKalmanFilter->Pk_1 + pKalmanFilter->Q;
//correction
pKalmanFilter->Kk = pKalmanFilter->Pkk_1 / (pKalmanFilter->Pkk_1 + pKalmanFilter->R);
pKalmanFilter->xk = pKalmanFilter->xkk_1 + pKalmanFilter->Kk * (zk - pKalmanFilter->xkk_1);
pKalmanFilter->Pk = (1.0f - pKalmanFilter->Kk) * pKalmanFilter->Pkk_1;
pKalmanFilter->xk_1 = pKalmanFilter->xk;
pKalmanFilter->Pk_1 = pKalmanFilter->Pk;
return pKalmanFilter->xk;
}
7.应用
这里自行构造了一组数据，并比较原始数据和经过卡尔曼滤波后的数据。
int main()
{
KALMAN_FILTER KalmanFilter;
float rawData[10]= {0.39f, 0.50f, 0.48f, 0.29f, 0.25f, 0.32f, 0.34f, 0.48f, 0.41f, 0.45f};
float filteredData[10] = {0};
unsigned int i = 0;
InitKalmanFilter(&KalmanFilter);
for (i = 0; i < 10; i++)
{
filteredData = CalcKalmanFilter(&KalmanFilter, rawData);
}
printf("raw data:\r\n");
for (i = 0; i < 10; i++)
{
printf("%.3f ", (double)rawData);
}
printf("\r\n");
printf("filtered data:\r\n");
for (i = 0; i < 10; i++)
{
printf("%.3f ", (double)filteredData);
}
printf("\r\n");
return 0;
}
8.输出结果
将输出数据复制到Excel中，绘制图像如下。
从图中可以明显看出经过卡尔曼滤波后的数据要比原始数据要平滑，且更符合实际情况。
————————————————
版权声明：本文为CSDN博主「propor」的原创文章，遵循CC 4.0 BY-SA版权协议，转载请附上原文出处链接及本声明。
原文链接：https://blog.csdn.net/propor/article/details/134810382
|
楼主
标题置顶
标题高亮
