卡尔曼滤波（Kalman Filter）
程序员文章站
2024-03-25 22:11:28
...
卡尔曼滤波是什么
卡尔曼滤波适用于估计一个动态系统的最优状态。即便是观测到的系统状态参数含有噪声，观测值不准确，卡尔曼滤波也能够完成对状态真实值的最优估计。网上大多数的教程讲到卡尔曼的数学公式推导，会让人很头疼，难以把握其中的主线和思想。所以我参考了国外一位学者的文章，讲述卡尔曼滤波的工作原理，然后编写了一个基于OpenCV的小程序给大家做一下说明。下面的这个视频请大家先直观地看看热闹吧~
角度跟踪视频
卡尔曼滤波能做什么
假设我们手头有一辆DIY的移动小车。这辆车的外形是这样的：
这辆车可以在荒野移动，为了便于对它进行控制，需要知道它的位置以及移动速度。所以，建立一个向量，用来存储小车的位置和速度
其实，一个系统的状态有很多，选择最关心的状态来建立这个状态向量是很重要的。例如，状态还有水库里面水位的高低、炼钢厂高炉内的温度、平板电脑上面指尖触碰屏幕的位置等等这些需要持续跟踪的物理量。好了，回归到正题，小车上面安装了GPS传感器，这个传感器的精度是10米。但是如果小车行驶的荒野上面有河流和悬崖的话，10米的范围就太大，很容易掉进去进而无法继续工作。所以，单纯靠GPS的定位是无法满足需求的。另外，如果有人说小车本身接收操控着发送的运动指令，根据车轮所转动过的圈数时能够知道它走了多远，但是方向未知，并且在路上小车打滑车轮空转的现象绝对是不可避免。所以，GPS以及车轮上面电机的码盘等传感器是间接地为我们提供了小车的信息，这些信息包含了很多的和不确定性。如果将所有这些信息综合起来，是否能够通过计算得到我们更想要的准确信息呢？答案是可以的！
卡尔曼滤波流程
使用OpenCV中Kalman编程的主要步骤
步骤一
Kalman这个类需要初始化变量：
转移矩阵，测量矩阵，控制向量(没有的话，就是0)，过程噪声协方差矩阵，测量噪声协方差矩阵，后验错误协方差矩阵，前一状态校正后的值，当前观察值。
void KalmanFilter::init(int dynamParams, int measureParams, int controlParams=0, int type=CV_32F)
//Parameters:
//dynamParams – Dimensionality of the state.
//measureParams – Dimensionality of the measurement.
//controlParams – Dimensionality of the control vector.
//type – Type of the created matrices that should be CV_32F or CV_64F.步骤二
调用kalman这个类的predict方法得到状态的预测值矩阵
预测状态的计算公式如下：
predicted state (x'(k)): x'(k)=A x(k-1)+B u(k)
其中x(k-1)为前一状态的校正值，第一个循环中在初始化过程中已经给定了，后面的循环中Kalman这个类内部会计算。A,B,u(k),也都是给定了的值。这样进过计算就得到了系统状态的预测值x'(k)了。
const Mat& KalmanFilter::predict(const Mat& control=Mat())
//Parameters: control – The optional input control步骤三：
调用kalman这个类的correct方法得到加入观察值校正后的状态变量值矩阵
其公式为：
corrected state (x(k)): x(k)=x'(k)+K(k) (z(k)-H x'(k))
其中x'(k)为步骤二算出的结果，z(k)为当前测量值，是我们外部测量后输入的向量。H为Kalman类初始化给定的测量矩阵。K(k)为Kalman增益，其计算公式为：
Kalman gain matrix (K(k)): K(k)=P'(k) Ht inv(H P'(k) Ht+R)
计算该增益所依赖的变量要么初始化中给定，要么在kalman理论中通过其它公式可以计算。
const Mat& KalmanFilter::correct(const Mat& measurement)
//Parameters: measurement – The measured system parameters经过步骤三后，我们又重新获得了这一时刻的校正值，后面就不断循环步骤二和步骤三即可完成Kalman滤波过程。
代码：
//1.kalman filter setup
const int stateNum = 4;
const int measureNum = 2;
KalmanFilter KF(stateNum, measureNum, 0);
Mat state(stateNum, 1, CV_32FC1); //state(x,y,detaX,detaY)
Mat processNoise(stateNum, 1, CV_32F);
Mat measurement = Mat::zeros(measureNum, 1, CV_32F); //measurement(x,y)
randn(state, Scalar::all(0), Scalar::all(0.1)); //随机生成一个矩阵，期望是0，标准差为0.1;
int matrix[] = {
1, 0, 1, 0,
0, 1, 0, 1,
0, 0, 1, 0,
0, 0, 0, 1};
KF.transitionMatrix = Mat(4, 4, CV_32F, matrix);//元素导入矩阵，按行;
//setIdentity: 缩放的单位对角矩阵;
//!< measurement matrix (H) 观测模型
setIdentity(KF.measurementMatrix);
//!< process noise covariance matrix (Q)
// wk 是过程噪声，并假定其符合均值为零，协方差矩阵为Qk(Q)的多元正态分布;
setIdentity(KF.processNoiseCov, Scalar::all(1e-1));
//!< measurement noise covariance matrix (R)
//vk 是观测噪声，其均值为零，协方差矩阵为Rk,且服从正态分布;
setIdentity(KF.measurementNoiseCov, Scalar::all(1e-3));
//!< priori error estimate covariance matrix (P'(k)): P'(k)=A*P(k-1)*At + Q)*/ A代表F: transitionMatrix
//估计值与观测值计算出的 真实值 对应的 协方差矩阵;
setIdentity(KF.errorCovPost, Scalar::all(1));
//!< corrected state (x(k)): x(k)=x'(k)+K(k)*(z(k)-H*x'(k))
//估计值与观测值计算出的 真实值
randn(KF.statePost, Scalar::all(0), Scalar::all(0.1));
Mat showImg(winWidth, winHeight, CV_8UC3);
for (;;)
{
setMouseCallback("Kalman", mouseEvent);
showImg.setTo(0);
Point statePt = Point((int)KF.statePost.at<float>(0), (int)KF.statePost.at<float>(1));
//2.kalman prediction
Mat prediction = KF.predict();
Point predictPt = Point((int)prediction.at<float>(0), (int)prediction.at<float>(1));
//3.update measurement
measurement.at<float>(0) = (float)mousePosition.x;
measurement.at<float>(1) = (float)mousePosition.y;
//4.update
Mat correct = KF.correct(measurement);
//cout<< measurement<<endl;
//randn( processNoise, Scalar(0), Scalar::all(sqrt(KF.processNoiseCov.at<float>(0, 0))));
//state = KF.transitionMatrix*state + processNoise;
//draw
circle(showImg, statePt, 5, CV_RGB(255, 0, 0), 1);//former point
circle(showImg, Point((int)correct.at<float>(0), (int)correct.at<float>(1)), 5, CV_RGB(0, 255, 0), 1);//predict point
circle(showImg, mousePosition, 5, CV_RGB(0, 0, 255), 1);//ture point
// CvFont font;//字体
// cvInitFont(&font, CV_FONT_HERSHEY_SCRIPT_COMPLEX, 0.5f, 0.5f, 0, 1, 8);
putText(showImg, "Red: Former Point", cvPoint(10, 30), FONT_HERSHEY_SIMPLEX, 1, Scalar::all(255));
putText(showImg, "Green: Predict Point", cvPoint(10, 60), FONT_HERSHEY_SIMPLEX, 1, Scalar::all(255));
putText(showImg, "Blue: Ture Point", cvPoint(10, 90), FONT_HERSHEY_SIMPLEX, 1, Scalar::all(255));
imshow("Kalman", showImg);
int key = waitKey(3);
if (key == 27)
{
break;
}
}
}
C++ Eigen 实现卡尔曼
kalman.hpp
#pragma once
/**
* Kalman filter implementation using Eigen. Based on the following
* introductory paper:
*
* http://www.cs.unc.edu/~welch/media/pdf/kalman_intro.pdf
*
* @author: Hayk Martirosyan
* @date: 2014.11.15
*/
#include <Eigen/Dense>
class KalmanFilterEigen {
public:
/**
* Create a Kalman filter with the specified matrices.
* A - System dynamics matrix
* C - Output matrix
* Q - Process noise covariance
* R - Measurement noise covariance
* P - Estimate error covariance
*/
KalmanFilterEigen(
double dt,
const Eigen::MatrixXd& A,
const Eigen::MatrixXd& C,
const Eigen::MatrixXd& Q,
const Eigen::MatrixXd& R,
const Eigen::MatrixXd& P
);
/**
* Create a blank estimator.
*/
KalmanFilterEigen();
/**
* Initialize the filter with initial states as zero.
*/
void init();
/**
* Initialize the filter with a guess for initial states.
*/
void init(double t0, const Eigen::VectorXd& x0);
/**
* Update the estimated state based on measured values. The
* time step is assumed to remain constant.
*/
void update(const Eigen::VectorXd& y);
/**
* Update the estimated state based on measured values,
* using the given time step and dynamics matrix.
*/
void update(const Eigen::VectorXd& y, double dt, const Eigen::MatrixXd A);
void changeQ(const Eigen::MatrixXd& q);
/**
* Return the current state and time.
*/
Eigen::VectorXd state() { return x_hat; };
double time() { return t; };
private:
// Matrices for computation
Eigen::MatrixXd A, C, Q, R, P, K, P0;
// System dimensions
int m, n;
// Initial and current time
double t0, t;
// Discrete time step
double dt;
// Is the filter initialized?
bool initialized;
// n-size identity
Eigen::MatrixXd I;
// Estimated states
Eigen::VectorXd x_hat, x_hat_new;
};kalman.cpp
/**
* Implementation of KalmanFilterEigen class.
*
* @author: Hayk Martirosyan
* @date: 2014.11.15
*/
#include <iostream>
#include <stdexcept>
#include "kalman.hpp"
KalmanFilterEigen::KalmanFilterEigen(
double dt,
const Eigen::MatrixXd& A,
const Eigen::MatrixXd& C,
const Eigen::MatrixXd& Q,
const Eigen::MatrixXd& R,
const Eigen::MatrixXd& P)
: A(A), C(C), Q(Q), R(R), P0(P),
m(C.rows()), n(A.rows()), dt(dt), initialized(false),
I(n, n), x_hat(n), x_hat_new(n)
{
I.setIdentity();
}
KalmanFilterEigen::KalmanFilterEigen() {}
void KalmanFilterEigen::init(double t0, const Eigen::VectorXd& x0) {
x_hat = x0;
P = P0;
this->t0 = t0;
t = t0;
initialized = true;
}
void KalmanFilterEigen::init() {
x_hat.setZero();
P = P0;
t0 = 0;
t = t0;
initialized = true;
}
void KalmanFilterEigen::changeQ(const Eigen::MatrixXd& q)
{
Q = q;
}
void KalmanFilterEigen::update(const Eigen::VectorXd& y) {
if (!initialized)
throw std::runtime_error("Filter is not initialized!");
x_hat_new = A * x_hat;
P = A*P*A.transpose() + Q;
K = P*C.transpose()*(C*P*C.transpose() + R).inverse();
x_hat_new += K * (y - C*x_hat_new);
P = (I - K*C)*P;
x_hat = x_hat_new;
t += dt;
}
void KalmanFilterEigen::update(const Eigen::VectorXd& y, double dt, const Eigen::MatrixXd A) {
this->A = A;
this->dt = dt;
update(y);
}
void kf_eigen()
{
int n = 3; // Number of states
int m = 1; // Number of measurements
double dt = 1.0 / 30; // Time step
Eigen::MatrixXd A(n, n); // System dynamics matrix
Eigen::MatrixXd C(m, n); // Output matrix
Eigen::MatrixXd Q(n, n); // Process noise covariance
Eigen::MatrixXd R(m, m); // Measurement noise covariance
Eigen::MatrixXd P(n, n); // Estimate error covariance
// Discrete LTI projectile motion, measuring position only
A << 1, dt, 0, 0, 1, dt, 0, 0, 1;
C << 1, 0, 0;
// Reasonable covariance matrices
Q << .05, .05, .0, .05, .05, .0, .0, .0, .0;
R << 5;
P << .1, .1, .1, .1, 10000, 10, .1, 10, 100;
std::cout << "A: \n" << A << std::endl;
std::cout << "C: \n" << C << std::endl;
std::cout << "Q: \n" << Q << std::endl;
std::cout << "R: \n" << R << std::endl;
std::cout << "P: \n" << P << std::endl;
// Construct the filter
KalmanFilterEigen kf(dt, A, C, Q, R, P);
// List of noisy position measurements (y)
std::vector<double> measurements = {
1.04202710058, 1.10726790452, 1.2913511148, 1.48485250951, 1.72825901034,
1.74216489744, 2.11672039768, 2.14529225112, 2.16029641405, 2.21269371128,
2.57709350237, 2.6682215744, 2.51641839428, 2.76034056782, 2.88131780617,
2.88373786518, 2.9448468727, 2.82866600131, 3.0006601946, 3.12920591669,
2.858361783, 2.83808170354, 2.68975330958, 2.66533185589, 2.81613499531,
2.81003612051, 2.88321849354, 2.69789264832, 2.4342229249, 2.23464791825,
2.30278776224, 2.02069770395, 1.94393985809, 1.82498398739, 1.52526230354,
1.86967808173, 1.18073207847, 1.10729605087, 0.916168349913, 0.678547664519,
0.562381751596, 0.355468474885, -0.155607486619, -0.287198661013, -0.602973173813
};
// Best guess of initial states
Eigen::VectorXd x0(n);
x0 << measurements[0], 0, -9.81;
kf.init(0, x0);
// Feed measurements into filter, output estimated states
double t = 0;
Eigen::VectorXd y(m);
std::cout << "t = " << t << ", " << "x_hat[0]: " << kf.state().transpose() << std::endl;
for (int i = 0; i < measurements.size(); i++) {
t += dt;
y << measurements[i];
kf.update(y);
std::cout << "t = " << t << ", " << "y[" << i << "] = " << y.transpose()
<< ", x_hat[" << i << "] = " << kf.state().transpose() << std::endl;
}
}
相关搜索
https://blog.csdn.net/heyijia0327/article/details/17487467
https://blog.csdn.net/NNNNNNNNNNNNY/article/details/53964823
https://zhuanlan.zhihu.com/p/21692854
https://blog.csdn.net/JasonDing1354/article/details/40537805