卡尔曼滤波的原理不多赘述,直接上代码(附件里也可以下载)
编译运行:
mkdir build
cd build
cmake ..
make
./kalman_test
- 头文件 — kalman_xv.h
#ifndef KALMAN_FILTER_H
#define KALMAN_FILTER_H
#include <eigen3/Eigen/Dense>
class LinearKalmanFilter
{
public:
LinearKalmanFilter()
{
is_initialized_ = false;
};
~LinearKalmanFilter();
void Initialization(Eigen::VectorXd x_in, double process_noise_std, double delta_t)
{
x_ = x_in;
Eigen::MatrixXd P_in(2, 2);
P_in << 1.0, 0.0,
0.0, 1.0;
P_ = P_in;
// 过程噪声矩阵
double process_noise_std_ = pow(process_noise_std, 2);
Eigen::MatrixXd Q_in(2, 2);
Q_in << process_noise_std_, 0.0,
0.0, process_noise_std_;
Q_ = Q_in;
// 观测矩阵
Eigen::MatrixXd H_in(1, 2);
H_in << 1.0, 0.0;
H_ = H_in;
// 观测噪声均值
Eigen::MatrixXd R_in(1, 1);
R_in << 1.0;
R_ = R_in;
// state transition matrix
// 状态转移矩阵
Eigen::MatrixXd F_in(2, 2);
F_in << 1.0, delta_t,
0.0, 1.0;
F_ = F_in;
this->is_initialized_ = true;
}
bool IsInitialized()
{
return this->is_initialized_;
}
void SetF(Eigen::MatrixXd F_in)
{
F_ = F_in;
}
void SetP(Eigen::MatrixXd P_in)
{
P_ = P_in;
}
void SetQ(Eigen::MatrixXd Q_in)
{
Q_ = Q_in;
}
void SetH(Eigen::MatrixXd H_in)
{
H_ = H_in;
}
void SetR(Eigen::MatrixXd R_in)
{
R_ = R_in;
}
// 预测
void Prediction()
{
x_ = F_ * x_;
Eigen::MatrixXd Ft = F_.transpose();
P_ = F_ * P_ * Ft + Q_;
}
// 更新
void MeasurementUpdate(const Eigen::VectorXd &z)
{
Eigen::VectorXd y = z - H_ * x_;
Eigen::MatrixXd S = H_ * P_ * H_.transpose() + R_;
Eigen::MatrixXd K = P_ * H_.transpose() * S.inverse(); // 计算卡尔曼增益
x_ = x_ + (K * y); // 状态更新 当前时刻通过卡尔曼滤波得到的目标估计量
int size = x_.size();
Eigen::MatrixXd I = Eigen::MatrixXd::Identity(size, size);
P_ = (I - K * H_) * P_; // 更新状态估计误差协方差
}
Eigen::VectorXd GetX()
{
return x_;
}
private:
bool is_initialized_; // flag of initialized
Eigen::VectorXd x_; // state vector 目标状态
Eigen::MatrixXd F_; // state transistion matrix 状态转移矩阵
Eigen::MatrixXd P_; // state covariance matrix 状态估计误差协方差
Eigen::MatrixXd Q_; // process covariance matrix 过程噪声矩阵
Eigen::MatrixXd H_; // mesurement matrix 测量/观测 矩阵
Eigen::MatrixXd R_; // mesurement covariance matrix 测量/观测 噪声均值
};
LinearKalmanFilter::~LinearKalmanFilter()
{
}
#endif
- 源文件 — kalman_xv.cpp
#include "kalman_xv.h"
#include <iostream>
#include <iomanip>
#include <string>
#include <stdio.h>
#include <vector>
#include <algorithm>
#include <cmath>
#include <fstream>
#include <chrono>
using namespace std;
int main()
{
double vel = 8.3; // 速度,单位m/s, 8.3m/s约30km/h
double delta_t = 0.1; // 时间间隔
int sample_time = 200;
double process_noise_std = 0.1; // 值越大越接近原始轨迹,平滑越弱
srand((unsigned int)time(NULL));
vector<double> xx;
// 模拟匀速直线运动的轨迹xx
for (int i = 0; i < sample_time; i++)
{
// 制造(-1~1)之间的随机误差
double random_num = pow(-1, rand()) * (double)(rand() % 10) / 10.0;
double tmp = i * vel * delta_t + random_num;
xx.push_back(tmp);
}
LinearKalmanFilter KF;
if (!KF.IsInitialized())
{
// cout << "*************** Before Initialize KF *************** KF.IsInitialized()=" << KF.IsInitialized() << endl;
Eigen::VectorXd x_in(2, 1);
x_in << xx[0], vel;
KF.Initialization(x_in, process_noise_std, delta_t);
// cout << "*************** After Initialize KF *************** KF.IsInitialized()=" << KF.IsInitialized() << endl;
}
// 将原始轨迹和预测轨迹保存成txt,后续可以用python或者matlab画对比曲线图
FILE *LOC_FILE = fopen("loc.txt", "w");
double m_x;
for (int i = 0; i < xx.size(); i++)
{
m_x = xx[i];
KF.Prediction();
// measurement value
Eigen::VectorXd z(1, 1);
z << m_x;
KF.MeasurementUpdate(z);
// result
Eigen::VectorXd x_out = KF.GetX();
std::cout << "真实位置:" << m_x << ",";
std::cout << "预测位置:" << x_out[0] << endl;
fprintf(LOC_FILE , "%f %f\n", m_x, x_out(0));
}
fclose(LOC_FILE );
}
- CmakeLists.txt
cmake_minimum_required(VERSION 3.0)
project(kalman_test)
add_definitions(-std=c++11)
set(CMAKE_CXX_STANDARD 11)
set(CMAKE_BUILD_TYPE Release)
include_directories("include/")
add_executable(kalman_test
src/kalman_xv.cpp
)