网站建设套餐电话,学校语言文字网站建设,中国建设银行网站设计评价,公司商标设计图一、概念介绍
卡尔曼滤波是一种高效率的递归滤波器(自回归滤波器), 它能够从一系列的不完全包含噪声的测量中#xff0c;估计动态系统的状态#xff0c;然而简单的卡尔曼滤波必须应用在符合高斯分布的系统中。
扩展卡尔曼滤波就是为了解决非线性问题#xff0c;普通卡尔曼…一、概念介绍
卡尔曼滤波是一种高效率的递归滤波器(自回归滤波器), 它能够从一系列的不完全包含噪声的测量中估计动态系统的状态然而简单的卡尔曼滤波必须应用在符合高斯分布的系统中。
扩展卡尔曼滤波就是为了解决非线性问题普通卡尔曼滤波只能应用于线性空间的问题。当系统为线性高斯模型时滤波器能给出最优的估计但是实际系统总是存在不同程度的非线性如平方、三角关系、开方等。对于非线性系统可以采用的一种方法是通过线性化方法将非线性系统转换为近似的线性系统即为EKF核心思想是围绕滤波值将非线性函数展开成泰勒级数并略去二阶及以上项得到一个近似的线性化模型然后应用卡尔曼滤波完成状态估计。
卡尔曼滤波是贝叶斯滤波的一个特例。当贝叶斯滤波中系统状态的置信度分布符合高斯分布时贝叶斯滤波卡尔曼滤波 二、数学表达
系统定义为
线性运动方程 非线性运动方程线性观测方程 非线性观测方程初始条件
并使用表示先验表示后验即最佳估计值。
1、线性卡尔曼滤波
预测
先验状态估计 先验误差协方差 校正
卡尔曼增益 更新
更新后验状态估计 更新后验误差协方差 2、扩展卡尔曼滤波
非线性方程线性化
非线性的状态方程和观测方程表示为如上所示符合正态分布的噪声数据在经过非线性转换后不再是正态分布的随机变量了此处使用近似处理 假设噪声符合上述正态分布而且它们为相互独立、正态分布的白噪声(即把噪声从非线性函数中拿出来)。在后续的线性化过程中这部分因为以上近似而产生的偏差会得到恢复 (近似地恢复) 。
对以上非线性方程(状态方程和观测方程)进行线性化具体方法为一阶泰勒展开忽略高阶项 其中为时刻对的雅克比矩阵
为时刻对的雅克比矩阵
为时刻对的雅克比矩阵
为时刻对的雅克比矩阵。 预测
先验状态估计 先验误差协方差 校正
卡尔曼增益 更新
更新后验状态估计 更新后验误差协方差 三、应用总结
滤波器针对的问题是如何通过数据来估计自身状态。因此可以有很多应用场景。
与线性不同扩展卡尔曼滤波器通常不是最佳估计器如果测量和状态转换模型都是线性的则它是最佳的因为在这种情况下扩展卡尔曼滤波器与常规滤波器相同。此外如果状态的初始估计是错误的或者如果过程建模不正确则由于其线性化滤波器可能会很快发散。扩展卡尔曼滤波器的另一个问题是估计的协方差矩阵往往会低估真实的协方差矩阵因此在不添加“稳定噪声”的情况下可能会在统计意义上 变得不一致 。更一般地我们应该考虑非线性滤波问题的无限维性质以及简单均值和方差-协方差估计器不足以完全表示最优滤波器。还应该注意的是即使对于非常简单的一维系统扩展卡尔曼滤波器也可能表现不佳。在这种情况下可以考虑 其他一般的非线性滤波方法。
EKF 方法是解决 SLAM 问题的一种经典方法其应用依赖于运动模型和观测模型的高斯噪声假设。EFK在数据融合上有很广泛的应用相对于优化的方法基于EKF的方法精度虽然较低但是计算效率更高。 四、代码实践
参考资料[2][3]中都含有二维平面小车状态估计的仿真非常值得看。这里放一下扩展卡尔曼滤波的C实现参考[4]。
kalman_filter.cpp
#include iostream
#include kalman_filter.h#define PI 3.14159265using namespace std;
using Eigen::MatrixXd;
using Eigen::VectorXd;KalmanFilter::KalmanFilter() {}KalmanFilter::~KalmanFilter() {}void KalmanFilter::Init(VectorXd x_in, MatrixXd P_in, MatrixXd F_in,MatrixXd H_in, MatrixXd R_in, MatrixXd Q_in) {x_ x_in;P_ P_in;F_ F_in;H_ H_in;R_ R_in;Q_ Q_in;
}void KalmanFilter::Predict() {//Use the state using the state transition matrixx_ F_ * x_;//Update the covariance matrix using the process noise and state transition matrixMatrixXd Ft F_.transpose();P_ F_ * P_ * Ft Q_;}void KalmanFilter::Update(const VectorXd z) {MatrixXd Ht H_.transpose();MatrixXd PHt P_ * Ht;VectorXd y z - H_ * x_;MatrixXd S H_ * PHt R_;MatrixXd K PHt * S.inverse();//Update Statex_ x_ (K * y);//Update covariance matrixlong x_size x_.size();MatrixXd I MatrixXd::Identity(x_size, x_size); P_ (I - K*H_) * P_;}void KalmanFilter::UpdateEKF(const VectorXd z) {float px x_(0);float py x_(1);float vx x_(2);float vy x_(3);//Convert the predictions into polar coordinatesfloat rho_p sqrt(px*px py*py);float theta_p atan2(py,px);if (rho_p 0.0001){cout Small prediction value - reassigning Rho_p to 0.0005 to avoid division by zero;rho_p 0.0001;}float rho_dot_p (px*vx py*vy)/rho_p;VectorXd z_pred VectorXd(3);z_pred rho_p, theta_p, rho_dot_p;VectorXd y z - z_pred;//Adjust the value of theta if it is outside of [-PI, PI]if (y(1) PI){y(1) y(1) - 2*PI;}else if (y(1) -PI){y(1) y(1) 2*PI;}MatrixXd Ht H_.transpose();MatrixXd PHt P_ * Ht;MatrixXd S H_ * PHt R_;MatrixXd K PHt * S.inverse();//Update Statex_ x_ (K * y);//Update covariance matrixlong x_size x_.size();MatrixXd I MatrixXd::Identity(x_size, x_size); P_ (I - K*H_) * P_;
}
kalman_filter.h
#ifndef KALMAN_FILTER_H_
#define KALMAN_FILTER_H_
#include Eigen/Denseclass KalmanFilter {
public:// state vectorEigen::VectorXd x_;// state covariance matrixEigen::MatrixXd P_;// state transition matrixEigen::MatrixXd F_;// process covariance matrixEigen::MatrixXd Q_;// measurement matrixEigen::MatrixXd H_;// measurement covariance matrixEigen::MatrixXd R_;/*** Constructor*/KalmanFilter();/*** Destructor*/virtual ~KalmanFilter();/*** Init Initializes Kalman filter* param x_in Initial state* param P_in Initial state covariance* param F_in Transition matrix* param H_in Measurement matrix* param R_in Measurement covariance matrix* param Q_in Process covariance matrix*/void Init(Eigen::VectorXd x_in, Eigen::MatrixXd P_in, Eigen::MatrixXd F_in,Eigen::MatrixXd H_in, Eigen::MatrixXd R_in, Eigen::MatrixXd Q_in);/*** Prediction Predicts the state and the state covariance* using the process model* param delta_T Time between k and k1 in s*/void Predict();/*** Updates the state by using standard Kalman Filter equations* param z The measurement at k1*/void Update(const Eigen::VectorXd z);/*** Updates the state by using Extended Kalman Filter equations* param z The measurement at k1*/void UpdateEKF(const Eigen::VectorXd z);};#endif /* KALMAN_FILTER_H_ */
参考资料
[1]扩展卡尔曼滤波(EKF)代码实战 - 小小市民的文章 - 知乎 https://zhuanlan.zhihu.com/p/161886906
[2]扩展卡尔曼滤波器实例与推导 - 西湖大学无人系统的文章 - 知乎 https://zhuanlan.zhihu.com/p/550160197
[3]扩展卡尔曼滤波算法及仿真实例_扩展卡尔曼滤波示例-CSDN博客
[4]GitHub - shazraz/Extended-Kalman-Filter: Implementation of an EKF in C
[5]网络资料
