文章目录
前言
题外话 ：
本文是博主对几个滤波算法整理的笔记之一，原本一篇，但由于CSDN篇幅限制分为了几个部分，包括：
RC滤波器：RC滤波器数学公式推导及软件算法实现
陷波滤波器：陷波滤波器的数学模型推导及算法实现
以及本文的卡尔曼滤波器。
原文PDF下载链接：https://mp.csdn.net/mp_download/manage/download/UpDetailed
回到本文主题：卡尔曼滤波器
参考来源于B站DR_CAN的卡尔曼滤波器系列讲解：https://www.bilibili.com/video/BV1ez4y1X7eR/?spm_id_from=333.788
讲解由浅入深，循序渐进，图文并茂，给人印象深刻。
卡尔曼滤波（Kalman filter）是一种高效率的递归滤波器（自回归滤波器），它能够从一系列的不完全及包含噪声的测量中，估计动态系统的状态。卡尔曼滤波会根据各测量量在不同时间下的值，考虑各时间下的联合分布，再产生对未知变数的估计，因此会比只以单一测量量为基础的估计方式要准。卡尔曼滤波得名自主要贡献者之一的鲁道夫·卡尔曼。
维基百科：卡尔曼滤波
在知乎看到一网友对卡尔曼滤波的本质理解，也是有一定道理的：
- 基于前一时间点的系统状态的概率分布给出当前时间点的系统状态的概率分布预测 P 1 \mathbf{P}_1 P1
- 基于当前时间点的观测量的概率分布
Q
\mathbf{Q}
Q给出另一组对当前时间点系统状态的概率分布预测
P
2
\mathbf{P}_2
P2
注意：如果传感器能直接把系统状态都观测了，那么我们直接用观测量的概率分布 Q \mathbf{Q} Q作为 P 2 \mathbf{P}_2 P2 就好了，但是有可能传感器观测的物理量量并不是直接的系统状态，而是其他物理量，这时我们需要通过做一个线性变换来获得 P 2 \mathbf{P}_2 P2 )
最后，得到两个关于当前时间点系统状态的概率分布的预测 P 1 \mathbf{P}_1 P1和 P 2 \mathbf{P}_2 P2 ，我们于是可以计算它们的联合概率分布。这个联合概率分布就是我们最终想要的校正过后的当前时间点的系统状态概率分布，有了它我们就可以进入下一个时间点继续迭代我们交替的预测-校正步骤了。
1、一些前置知识
递归算法（Recursive Processing
数据融合（Data fusion）
相关数学基础
状态空间方程与观测器
2、状态空间方程
3、估计模型与测量估计的数据融合
4、卡尔曼增益的推导（Kalman gain）
5、先验估计协方差的求解
6、后验估计协方差的化简
7、标准卡尔曼滤波算法实现及验证
一维标准卡尔曼算法实现及验证
#include <iostream>
#include <fstream>
#include <vector>
#include <cstdlib> // For random number generation
#include <ctime> // For seeding the random number generator
using namespace std;
typedef struct
{
float P_k_prev; //上次估算协方差
float P_k_now; //当前估算协方差
float out_prev; //上一时刻卡尔曼滤波器输出
float out_new; //当前时刻卡尔曼滤波器输出
float Kk; //卡尔曼增益
float Q; //过程噪声协方差
float R; //观测噪声协方差
}klman_filter_s;//Kalman Filter parameter
klman_filter_s klman_filter;
void kalman_filter_init(klman_filter_s* klman_filter)
{
klman_filter->P_k_prev = 0.02; //非0
klman_filter->P_k_now = 0;
klman_filter->out_prev = 0;
klman_filter->out_new = 0;
klman_filter->Kk = 0;
klman_filter->Q = 0.001;
klman_filter->R = 0.1;
}
void kalman_filter_calc(klman_filter_s * klman_filter,float input)
{
/*
*先验估计噪声的协方差更新:
*k时刻的先验估计噪声协方差 = k-1时刻后验估计噪声协方差 + 过程噪声协方差
*/
klman_filter->P_k_now = klman_filter->P_k_prev + klman_filter->Q;
/* 卡尔曼增益计算：
* k时刻的卡尔曼增益 = k时刻先验估计噪声协方差/(k时刻先验估计噪声协方差 + 测量噪声协方差)
*/
klman_filter->Kk = klman_filter->P_k_now / (klman_filter->P_k_now + klman_filter->R);
/*
* 模型估计与测量估计的数据融合:(k时刻先验估计值=k-1时刻估计值，过程噪声为0）
* k时刻估计值 = k-1时刻的估计值 + 卡尔曼增益 *（输入值 - k-1时刻的估计值）
*/
klman_filter->out_new = klman_filter->out_prev + klman_filter->Kk * (input - klman_filter->out_prev);
/* 后验估计噪声的协方差更新:
* k时刻的后验估计噪声协方差 = （ 1 - klman_filter->Kk ）* k时刻的先验估计噪声协方差
* 在下一时刻后验估计噪声协方差 = 当前时刻的后验估计噪声协方差
*/
klman_filter->P_k_prev = (1 - klman_filter->Kk) * klman_filter->P_k_now;
klman_filter->out_prev = klman_filter->out_new;
}
void saveToCSV(const vector<double>& data, const string& filename) {
ofstream file(filename);
if (file.is_open()) {
for (const double& value : data) {
file << value << endl;
}
file.close();
cout << "Data saved to " << filename << endl;
}
else {
cerr << "Unable to open the file: " << filename << endl;
}
}
int main() {
int numSamples = 1000; // Number of samples
vector<double> inputSignal(numSamples);
vector<double> noisySignal(numSamples);
vector<double> filteredSignal(numSamples);
// Seed the random number generator
srand(time(nullptr));
// Generate input signal with noise
for (int i = 0; i < numSamples; ++i) {
if (numSamples<100)
{
inputSignal[i] = ((float)rand() / RAND_MAX - 0.5);
}
else if (numSamples>=100 && numSamples < 500) {
inputSignal[i] = 0.5 * ((float)rand() / RAND_MAX - 0.5);
}
else if (numSamples >= 500 && numSamples < 700) {
inputSignal[i] = 1.5 * ((float)rand() / RAND_MAX - 0.5);
}
else
{
inputSignal[i] = 5 * ((float)rand() / RAND_MAX - 0.5);
}
}
kalman_filter_init(&klman_filter);
for (int i = 0; i < numSamples; ++i) {
double Xn = inputSignal[i];
kalman_filter_calc(&klman_filter, Xn);
filteredSignal[i] = klman_filter.out_new;
}
// Save data to CSV files
std::string filename1 = "KPF_before_Data.csv";
saveToCSV(inputSignal, "input_signal.csv");
std::string filename2 = "KPF_after_Data.csv";
saveToCSV(filteredSignal, "filtered_signal.csv");
return 0;
}
二维标准卡尔曼算法实现及验证
为了方便验证，我将原本可以模块化的矩阵运行函数写在了一个文件里，以下是源码：
#include <fstream>
#include <vector>
#include <cstdlib> // For random number generation
#include <ctime> // For seeding the random number generator
using namespace std;
/************************************该部分可以封装成模块*******************************************/
//申请最大矩阵内存
#define MATSIZE 50
//结构体
typedef struct Matrix
{
int row, col; //row为行,col
Matrix() {}; //这是默认构造函数。在这里，构造函数没有实际操作。
Matrix(int _row, int _col) :row(_row), col(_col) {} //这是带参数的构造函数。
//当你创建一个带有行数和列数参数的 Matrix 对象时，
//这个构造函数会设置矩阵的行数和列数。
double data[MATSIZE];
};
void InitMatrix(Matrix* matrix, int col, int row, int value) //初始化一个矩阵
{
if (col > 0 && row > 0)
{
matrix->col = col;
matrix->row = row;
memset(matrix->data, value, sizeof(double) * col * row);
return;
}
else
return;
}
void ValueMatrix(Matrix* matrix, double* array) //给矩阵赋值
{
if (matrix->data != NULL)
{
memcpy(matrix->data, array, matrix->col * matrix->row * sizeof(double));
}
}
int SizeMatrix(Matrix* matrix)
{
return matrix->col * matrix->row;
}
void CopyMatrix(Matrix* matrix_A, Matrix* matrix_B)
{
matrix_B->col = matrix_A->col;
matrix_B->row = matrix_A->row;
memcpy(matrix_B->data, matrix_A->data, SizeMatrix(matrix_A) * sizeof(double));
}
void PrintMatrix(Matrix* matrix)
{
for (int i = 0; i < SizeMatrix(matrix); i++)
{
//printf("%lf\t", matrix->data[i]);
cout << "matrix ：" << matrix->data[i] << endl;
if ((i + 1) % matrix->row == 0)
//printf("\n");
cout << endl;
}
}
/* 加法 */
void AddMatrix(Matrix matrix_A, Matrix matrix_B, Matrix* matrix_C)
{
if (matrix_A.col == matrix_B.col && matrix_A.row == matrix_B.row)
{
matrix_C->row = matrix_A.row;
matrix_C->col = matrix_B.col;
for (int i = 0; i < matrix_A.row; i++)
{
for (int j = 0; j < matrix_A.col; j++)
{
matrix_C->data[i * matrix_C->col + j] = \
matrix_A.data[i * matrix_A.col + j] + matrix_B.data[i * matrix_A.col + j];
}
}
}
else
{
//printf("不可相加\n");
cout << "不可相加" << endl;
}
}
/* 减法 */
void SubMatrix(Matrix matrix_A, Matrix matrix_B, Matrix* matrix_C)
{
if (matrix_A.col == matrix_B.col && matrix_A.row == matrix_B.row)
{
matrix_C->row = matrix_A.row;
matrix_C->col = matrix_A.col;
for (int i = 0; i < matrix_A.row; i++)
{
for (int j = 0; j < matrix_A.col; j++)
{
matrix_C->data[i * matrix_C->col + j] = \
matrix_A.data[i * matrix_A.col + j] - matrix_B.data[i * matrix_A.col + j];
}
}
}
else
{
//printf("不可相减\n");
cout << "不可相减" << endl;
}
}
/* 乘法 */
void MulMatrix(Matrix matrix_A, Matrix matrix_B, Matrix* matrix_C)
{
matrix_C->row = matrix_A.row;
matrix_C->col = matrix_B.col;
memset(matrix_C->data, 0, MATSIZE * sizeof(double));
if (matrix_A.col == matrix_B.row) //列==行
{
for (int i = 0; i < matrix_A.row; i++)
{
for (int j = 0; j < matrix_B.col; j++)
{
for (int k = 0; k < matrix_A.row; k++)
{
matrix_C->data[i * matrix_C->col + j] += \
matrix_A.data[i * matrix_A.col + k] * matrix_B.data[k * matrix_B.col + j];
}
}
}
}
else
{
//printf("不可相乘\n");
cout << "不可相乘" << endl;
}
}
//逆矩阵
void InvMatrix(Matrix matrix_A, Matrix* matrix_C)
{
matrix_C->col = matrix_A.col;
matrix_C->row = matrix_A.row;
double f, k;
if (matrix_A.col == 2 && matrix_A.row == 2) //列==行
{
f = matrix_A.data[3] * matrix_A.data[0] - matrix_A.data[1] * matrix_A.data[2];
if (f == 0) {
//printf("不可逆\n");
cout << "不可逆" << endl;
return;
}
else
k = 1 / f;
matrix_C->data[0] = matrix_A.data[3] * k;
matrix_C->data[1] = -matrix_A.data[1] * k;
matrix_C->data[2] = -matrix_A.data[2] * k;
matrix_C->data[3] = matrix_A.data[0] * k;
}
else
{
//printf("不可逆\n");
cout << "不可逆" << endl;
return;
}
}
/* 矩阵转置
* 条件为方阵
*/
void TransMatrix(Matrix matrix, Matrix* matrixTemp)
{
matrixTemp->col = matrix.row;
matrixTemp->row = matrix.col;
if (matrix.col > 0 && matrix.row > 0)
{
for (int i = 0; i < matrix.col; i++)
{
for (int j = 0; j < matrix.row; j++)
{
matrixTemp->data[i * matrixTemp->col + j] = matrix.data[j * matrix.col + i];
}
}
return;
}
else
{
//printf("转置的矩阵必须为方阵\n");
cout << "转置的矩阵必须为方阵" << endl;
}
}
/************************************该部分可以封装成模块*******************************************/
//Matrix A(2,2);
//typedef struct
//{
// Matrix A; //状态矩阵
// Matrix B; //输入矩阵
// Matrix H; //观测矩阵
//
// Matrix P_k_prev; //上次估算协方差
// Matrix P_k_now; //当前估算协方差
// Matrix input; //输入
// Matrix out_prev; //上一时刻卡尔曼滤波器输出
// Matrix out_new; //当前时刻卡尔曼滤波器输出
// Matrix Kk; //卡尔曼增益
// Matrix Q; //过程噪声协方差
// Matrix R; //观测噪声协方差
//}klman_filter_s;//Kalman Filter parameter
//
//klman_filter_s klman_filter;
/* 状态转移矩阵 */
double a[4] = {
1,1,
0,1
};
/* 过程噪声协方差矩阵 */
double q[4] = {
0.1,0,
0,0.1
};
/* 观测噪声协方差矩阵 */
double r[4] = {
1,0,
0,1
};
/* 观测噪声协方差矩阵 */
double h[4] = {
1,0,
0,1
};
/* 输入矩阵 */
double b[4] = {
1,0,
0,1
};
/* 初始位置与速度 */
double X0[2] = {
0,
1
};
/* 状态估计协方差矩阵P初始化 */
double P0[4] = {
1,0,
0,1
};
/* 单位矩阵 */
double i[4] = {
1,0,
0,1
};
/* 零矩阵 */
double o[4] = {
0,0,
0,0
};
/* 零矩阵 */
double o1[2] = {
0,0
};
Matrix Ii(2,2);
Matrix A(2, 2);
Matrix B(2, 2);
Matrix H(2, 2);
Matrix P_k_prev(2, 2);
Matrix P_k_now(2, 2);
Matrix input(2, 1);
Matrix out_prev(2, 1);
Matrix out_new(2, 1);
Matrix Kk(2, 1);
Matrix Q(2, 2);
Matrix R(2, 2);
void kalman_filter_init(void)
{
/* 初始化矩阵并赋值 */
ValueMatrix(&A, a);
ValueMatrix(&B, b);
ValueMatrix(&H, h);
ValueMatrix(&P_k_prev, P0);
ValueMatrix(&P_k_now, o);
ValueMatrix(&input, o1);
ValueMatrix(&out_prev, o1);
ValueMatrix(&out_new, o1);
ValueMatrix(&Kk, o1);
ValueMatrix(&Q, q);
ValueMatrix(&R, r);
ValueMatrix(&Ii, i);
}
void kalman_filter_calc(float xinput,float yinput)
{
Matrix temp1;
Matrix temp2;
Matrix temp3;
Matrix temp4;
Matrix temp5;
Matrix temp6;
/* 测量值赋值*/
input.data[0] = xinput;
input.data[1] = yinput;
/*
*先验估计噪声的协方差更新:
*k时刻的先验估计噪声协方差 = k-1时刻后验估计噪声协方差 + 过程噪声协方差
*/
MulMatrix(A, P_k_prev, &temp1);// A * P(k-1)
TransMatrix(A, &temp2); // A 的转置
MulMatrix(temp1, temp2, &temp3); // A * P(k-1)*A^T
AddMatrix(temp3, Q, &P_k_now); //P(k)^-=A * P(k-1)*A^T+Q
/* 卡尔曼增益计算：
* k时刻的卡尔曼增益 = k时刻先验估计噪声协方差/(k时刻先验估计噪声协方差 + 测量噪声协方差)
*/
TransMatrix(H, &temp1); // H 的转置
MulMatrix(P_k_now, temp1, &temp2); // P(k)^*H^T
MulMatrix(H, P_k_now, &temp3); // H*P(k)^-
MulMatrix(temp3, temp1, &temp4); // H*P(k)^-*H^T
AddMatrix(temp4, R, &temp5); //H*P(k)^-*H^T+R
InvMatrix(temp5, &temp6); //求（H*P(k)^-*H^T+R）的逆
MulMatrix(temp2, temp6, &Kk); // Kk
/*
* 模型估计与测量估计的数据融合:(k时刻先验估计值=k-1时刻估计值，过程噪声为0）
* k时刻估计值 = k-1时刻的估计值 + 卡尔曼增益 *（输入值 - k-1时刻的估计值）
*/
MulMatrix(H, out_prev, &temp1); // H*X(k-1)
SubMatrix(input, temp1, &temp2); //Z(k)-H*X(k-1)
MulMatrix(Kk, temp2, &temp3); //K(k)*( Z(k)-H*X(k-1))
AddMatrix(out_prev, temp3,&out_new); // X(k-1)+ K(k)*( Z(k)-H*X(k-1))
/* 后验估计噪声的协方差更新:
* k时刻的后验估计噪声协方差 = （ 1 - klman_filter->Kk ）* k时刻的先验估计噪声协方差
* 在下一时刻后验估计噪声协方差 = 当前时刻的后验估计噪声协方差
*/
MulMatrix(Kk, H, &temp1); // K(k)*H
SubMatrix(Ii, temp1, &temp2); //I-K(k)*H
MulMatrix(temp2, P_k_now, &P_k_prev); // (I-K(k)*H)*P(k)^-
CopyMatrix(&out_new, &out_prev); // out_prev = out_new;
}
void saveToCSV(const vector<double>& data, const string& filename) {
ofstream file(filename);
if (file.is_open()) {
for (const double& value : data) {
file << value << endl;
}
file.close();
cout << "Data saved to " << filename << endl;
}
else {
cerr << "Unable to open the file: " << filename << endl;
}
}
int main() {
int numSamples = 1000; // Number of samples
vector<double> inputSignalx(numSamples);
vector<double> inputSignaly(numSamples);
vector<double> noisySignal(numSamples);
vector<double> filteredSignalx(numSamples);
vector<double> filteredSignaly(numSamples);
// Seed the random number generator
srand(time(nullptr));
// Generate input signal with noise
for (int i = 0; i < numSamples; ++i) {
if (numSamples<100)
{
inputSignalx[i] = ((float)rand() / RAND_MAX - 0.5);
}
else if (numSamples>=100 && numSamples < 500) {
inputSignalx[i] = 0.5 * ((float)rand() / RAND_MAX - 0.5);
}
else if (numSamples >= 500 && numSamples < 700) {
inputSignalx[i] = 1.5 * ((float)rand() / RAND_MAX - 0.5);
}
else
{
inputSignalx[i] = 5 * ((float)rand() / RAND_MAX - 0.5);
}
}
for (int i = 0; i < numSamples; ++i) {
if (numSamples < 100)
{
inputSignaly[i] = ((float)rand() / RAND_MAX - 0.5);
}
else if (numSamples >= 100 && numSamples < 500) {
inputSignaly[i] = 0.5 * ((float)rand() / RAND_MAX - 0.5);
}
else if (numSamples >= 500 && numSamples < 700) {
inputSignaly[i] = 1.5 * ((float)rand() / RAND_MAX - 0.5);
}
else
{
inputSignaly[i] = 5 * ((float)rand() / RAND_MAX - 0.5);
}
}
kalman_filter_init();
for (int i = 0; i < numSamples; ++i) {
double xn = inputSignalx[i];
double yn = inputSignaly[i];
kalman_filter_calc(xn,yn);
filteredSignalx[i] = out_new.data[0];//
filteredSignaly[i] = out_new.data[1];//
}
// Save data to CSV files
std::string filename1 = "KPF_before_Data.csv";
saveToCSV(inputSignalx, "input_x_signal.csv");
saveToCSV(inputSignaly, "input_y_signal.csv");
std::string filename2 = "KPF_after_Data.csv";
saveToCSV(filteredSignalx, "filtered_signalx.csv");
saveToCSV(filteredSignaly, "filtered_signaly.csv");
return 0;
}
速度估计结果：
位置估计结果：
以上初步实现估计，参数可能有待优化。
482
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
