卡尔曼滤波原理及c++代码实现

这篇具有很好参考价值的文章主要介绍了卡尔曼滤波原理及c++代码实现。希望对大家有所帮助。如果存在错误或未考虑完全的地方,请大家不吝赐教,您也可以点击"举报违法"按钮提交疑问。

最近重新看到了卡尔曼滤波,想把这部分的原理整理写下来。供需要的人理解和学习。
一键三连,点赞收藏关注!

一、定义:

卡尔曼滤波(Kalman filtering)是一种利用线性系统状态方程,通过系统输入输出观测数据,对系统状态进行最优估计的算法,“optimal recursivedata processing algorithm(最优化自回归数据处理算法)”。

  • 卡尔曼滤波不要求信号和噪声都是平稳过程的假设条件。对于每个时刻的系统扰动和观测误差(即噪声),只要对它们的统计性质作某些适当的假定,通过对含有噪声的观测信号进行处理,就能在平均的意义上,求得误差为最小的真实信号的估计值。因此,自从卡尔曼滤波理论问世以来,在通信系统、电力系统、航空航天、环境污染控制、工业控制、雷达信号处理等许多部门都得到了应用,取得了许多成功应用的成果。例如在图像处理方面,应用卡尔曼滤波对由于某些噪声影响而造成模糊的图像进行复原。在对噪声作了某些统计性质的假定后,就可以用卡尔曼的算法以递推的方式从模糊图像中得到均方差最小的真实图像,使模糊的图像得到复原。

二、性质:

(1)卡尔曼滤波是一个算法,它适用于线性、离散和有限维系统。每一个有外部变量的自回归移动平均系统(ARMAX)或可用有理传递函数表示的系统都可以转换成用状态空间表示的系统,从而能用卡尔曼滤波进行计算。

(2)任何一组观测数据都无助于消除x(t)的确定性。增益K(t)也同样地与观测数据无关。

(3)当观测数据和状态联合服从高斯分布时用卡尔曼递归公式计算得到的是高斯随机变量的条件均值和条件方差,从而卡尔曼滤波公式给出了计算状态的条件概率密度的更新过程线性最小方差估计,也就是最小方差估计。

三、算法介绍

卡尔曼滤波器算法 :

  • 在这一部分,我们就来描述源于Dr Kalman 的卡尔曼滤波器。下面的描述,会涉及一些基本的概念知识,包括概率(Probability),随机变量(Random Variable),高斯或正态分配(Gaussian Distribution)还有State-space Model等等。但对于卡尔曼滤波器的详细证明,这里不能一一描述。
  • 首先,我们先要引入一个离散控制过程的系统。该系统可用一个线性随机微分方程(Linear Stochastic Difference equation)来描述:
X(k)=A X(k-1)+B U(k)+W(k)

再加上系统的测量值:

Z(k)=H X(k)+V(k)
  • 上两式子中,X(k)是k时刻的系统状态,U(k)是k时刻对系统的控制量。A和B是系统参数,对于多模型系统,他们为矩阵。Z(k)是k时刻的测量值,H是测量系统的参数,对于多测量系统,H为矩阵。W(k)和V(k)分别表示过程和测量的噪声。他们被假设成高斯白噪声(White Gaussian Noise),他们的协方差(covariance)分别是Q,R(这里我们假设他们不随系统状态变化而变化)。
  • 对于满足上面的条件(线性随机微分系统,过程和测量都是高斯白噪声),卡尔曼滤波器是最优的信息处理器。下面我们结合他们的协方差来估算系统的最优化输出(类似上一节那个温度的例子)。
  • 首先我们要利用系统的过程模型,来预测下一状态的系统。假设现在的系统状态是k,根据系统的模型,可以基于系统的上一状态而预测出现在状态:
X(k|k-1)=A X(k-1|k-1)+B U(k) ……….. (1)

式(1)中,X(k|k-1)是利用上一状态预测的结果,X(k-1|k-1)是上一状态最优的结果,U(k)为现在状态的控制量,如果没有控制量,它可以为0。
到现在为止,我们的系统结果已经更新了,可是,对应于X(k|k-1)的协方差还没更新。我们用P表示协方差(covariance):

P(k|k-1)=A P(k-1|k-1) A’+Q ……… (2)

式(2)中,P(k|k-1)是X(k|k-1)对应的协方差,P(k-1|k-1)是X(k-1|k-1)对应的协方差,A’表示A的转置矩阵,Q是系统过程的协方差。式子1,2就是卡尔曼滤波器5个公式当中的前两个,也就是对系统的预测。
现在我们有了现在状态的预测结果,然后我们再收集现在状态的测量值。结合预测值和测量值,我们可以得到现在状态(k)的最优化估算值X(k|k):

X(k|k)= X(k|k-1)+Kg(k) (Z(k)-H X(k|k-1)) ……… (3)

其中Kg为卡尔曼增益(Kalman Gain):

Kg(k)= P(k|k-1) H’ / (H P(k|k-1) H’ + R) ……… (4)

到现在为止,我们已经得到了k状态下最优的估算值X(k|k)。但是为了要令卡尔曼滤波器不断的运行下去直到系统过程结束,我们还要更新k状态下X(k|k)的协方差:

P(k|k)=(I-Kg(k) H)P(k|k-1) ……… (5)

其中I 为1的矩阵,对于单模型单测量,I=1。当系统进入k+1状态时,P(k|k)就是式子(2)的P(k-1|k-1)。这样,算法就可以自回归的运算下去。

  • 卡尔曼滤波器的原理基本描述了,式子1,2,3,4和5就是他的5 个基本公式。

那么,我们可以理解:使用上一时刻的最优结果预测这一时刻的预测值,同时使用这一时刻观测值(传感器测得的数据)修正这一时刻预测值,得到这一时刻的最优结果。

我们可以不需要累计很久的数据,我们只用上次得到的数据和本次测量的数据即可推算本次数据估计的结果。

c++代码kalman滤波

/** 
 * Write a function 'filter()' that implements a multi-
 *   dimensional Kalman Filter for the example given
 */

#include <iostream>
#include <vector>
// Eigen 部分
#include <Eigen/Core>
#include <Eigen/Dense>

using std::cout;
using std::endl;
using std::vector;
using Eigen::VectorXd;
using Eigen::MatrixXd;

// Kalman Filter variables
VectorXd x;	// object state 障碍物状态矩阵
MatrixXd P;	// object covariance matrix 障碍物不确定性协方差
VectorXd u;	// external motion 外部的运动
MatrixXd F; // state transition matrix 状态转移矩阵
MatrixXd H;	// measurement matrix 测量矩阵
MatrixXd R;	// measurement covariance matrix 测量协方差矩阵
MatrixXd I; // Identity matrix 单位矩阵
MatrixXd Q;	// process covariance matrix 过程协方差矩阵

vector<VectorXd> measurements; // 测量值
void filter(VectorXd &x, MatrixXd &P); // 滤波函数


int main() {
  /**
   * Code used as example to work with Eigen matrices
   */
  // design the KF with 1D motion
  x = VectorXd(2); // 初始化障碍物状态矩阵 第一个为位置,第二个为速度
  x << 0, 0;

  P = MatrixXd(2, 2); // 初始化不确定性协方差矩阵, 刚开始,位置(0,0)的不确定性为1000,速度的不确定性为1000
  P << 1000, 0, 0, 1000;

  u = VectorXd(2); // 运动误差
  u << 0, 0;

  F = MatrixXd(2, 2); // 状态转移矩阵初始化
  F << 1, 1, 0, 1;

  H = MatrixXd(1, 2); // 测量矩阵
  H << 1, 0;

  R = MatrixXd(1, 1); // 测量协方差矩阵
  R << 1;

  I = MatrixXd::Identity(2, 2); // 单位矩阵

  Q = MatrixXd(2, 2); // 过程协方差矩阵
  Q << 0, 0, 0, 0;

  // create a list of measurements
  VectorXd single_meas(1); 
  single_meas << 1;
  measurements.push_back(single_meas);
  single_meas << 2;
  measurements.push_back(single_meas);
  single_meas << 3;
  measurements.push_back(single_meas);

  // call Kalman filter algorithm
  filter(x, P);

  return 0;
}


void filter(VectorXd &x, MatrixXd &P) {

  for (unsigned int n = 0; n < measurements.size(); ++n) {

    VectorXd z = measurements[n];
    // TODO: YOUR CODE HERE
    /**
     * KF Measurement update step
     */
    VectorXd y = z - H * x;
    MatrixXd Ht = H.transpose();
    MatrixXd S = H * P * Ht + R;
    MatrixXd Si = S.inverse();
    MatrixXd K = P * Ht * Si;

    // new state
    x = x + (K * y);
    P = (I - K * H) * P;

    /**
     * KF Prediction step
     */
    x = F * x + u;
    MatrixXd Ft = F.transpose();
    P = F * P * Ft + Q;

    cout << "x=" << endl
         << x << endl;
    cout << "P=" << endl
         << P << endl;
  }
}

代码可以调试看看参数的具体数值,更便于理解算法。
有后续的知识会继续添加。
未完待续!

引用博客:
传送门–>大佬文章来源地址https://www.toymoban.com/news/detail-733945.html

到了这里,关于卡尔曼滤波原理及c++代码实现的文章就介绍完了。如果您还想了解更多内容,请在右上角搜索TOY模板网以前的文章或继续浏览下面的相关文章,希望大家以后多多支持TOY模板网!

本文来自互联网用户投稿,该文观点仅代表作者本人,不代表本站立场。本站仅提供信息存储空间服务,不拥有所有权,不承担相关法律责任。如若转载,请注明出处: 如若内容造成侵权/违法违规/事实不符,请点击违法举报进行投诉反馈,一经查实,立即删除!

领支付宝红包 赞助服务器费用

相关文章

  • 40行MATLAB代码实现卡尔曼滤波-简单易懂

    最近学习了卡尔曼滤波,体会到了数据融合下进行最优估计的思想。如果你也是小白,可以通过这个例子自己动手感受数据融合。 学习资料参考B站大神DR_CAN博士,连接: 【卡尔曼滤波器】直观理解与二维实例 基于上述视频中Excel的例子,使用MATLAB编写了一个简单的卡尔曼滤

    2024年02月04日
    浏览(49)
  • 卡尔曼滤波器的定义,实例和代码实现

    卡尔曼滤波器(Kalman filter)是一种高效的递归滤波器, 能够从一系列包含噪音的测量值中估计动态系统的状态. 因为不需要存储历史状态, 没有复杂计算, 非常适合在资源有限的嵌入式系统中使用. 常用于飞行器的导引, 导航及控制, 机械和金融中的时间序列分析, 轨迹最佳化等. 卡

    2024年03月09日
    浏览(44)
  • 【状态估计】基于卡尔曼滤波器和扩展卡尔曼滤波器用于 INS/GNSS 导航、目标跟踪和地形参考导航研究(Matlab代码实现)

    💥💥💞💞 欢迎来到本博客 ❤️❤️💥💥 🏆博主优势: 🌞🌞🌞 博客内容尽量做到思维缜密,逻辑清晰,为了方便读者。 ⛳️ 座右铭: 行百里者,半于九十。 📋📋📋 本文目录如下: 🎁🎁🎁 目录 💥1 概述 📚2 运行结果  2.1 算例1 ​ 2.2 算例2  2.3 算例3 🎉3 参

    2024年02月11日
    浏览(49)
  • 卡尔曼滤波算法原理及示例

      例程:物体做匀速运动每秒运动1m,观测器观测方差为1m

    2024年02月15日
    浏览(39)
  • 使用扩展卡尔曼滤波(EKF)融合激光雷达和雷达数据(Matlab代码实现)

    💥💥💞💞 欢迎来到本博客 ❤️❤️💥💥 🏆博主优势: 🌞🌞🌞 博客内容尽量做到思维缜密,逻辑清晰,为了方便读者。 ⛳️ 座右铭: 行百里者,半于九十。 📋📋📋 本文目录如下: 🎁🎁🎁 目录 💥1 概述 📚2 运行结果 🎉3 参考文献 🌈4 Matlab代码实现 大多数自

    2024年02月09日
    浏览(52)
  • 【状态估计】变分贝叶斯近似的递归噪声自适应卡尔曼滤波(Matlab代码实现)

    💥💥💞💞 欢迎来到本博客 ❤️❤️💥💥 🏆博主优势: 🌞🌞🌞 博客内容尽量做到思维缜密,逻辑清晰,为了方便读者。 ⛳️ 座右铭: 行百里者,半于九十。 📋📋📋 本文目录如下: 🎁🎁🎁 目录 💥1 概述 📚2 运行结果 🎉3 参考文献 🌈4 Matlab代码及文献 文献来

    2024年02月09日
    浏览(42)
  • 【图像处理 】卡尔曼滤波器原理

    目录 一、说明 二、它是什么? 2.1 我们可以用卡尔曼滤波器做什么? 2.2 卡尔曼滤波器如何看待您的问题

    2024年02月06日
    浏览(49)
  • 【状态估计】粒子滤波器、Σ点滤波器和扩展/线性卡尔曼滤波器研究(Matlab代码实现)

    💥💥💞💞 欢迎来到本博客 ❤️❤️💥💥 🏆博主优势: 🌞🌞🌞 博客内容尽量做到思维缜密,逻辑清晰,为了方便读者。 ⛳️ 座右铭: 行百里者,半于九十。 📋📋📋 本文目录如下: 🎁🎁🎁 目录 💥1 概述 📚2 运行结果 2.1 扩展卡尔曼滤波 2.2 线性卡尔曼滤波 

    2024年02月09日
    浏览(41)
  • 【状态估计】基于线性卡尔曼滤波器和粒子滤波器无人机估计地形高度(Matlab代码实现)

      💥💥💞💞 欢迎来到本博客 ❤️❤️💥💥 🏆博主优势: 🌞🌞🌞 博客内容尽量做到思维缜密,逻辑清晰,为了方便读者。 ⛳️ 座右铭: 行百里者,半于九十。 📋📋📋 本文目录如下: 🎁🎁🎁 目录 💥1 概述 📚2 运行结果 🎉3 参考文献 🌈4 Matlab代码实现 本文模

    2024年02月16日
    浏览(44)
  • 集合卡尔曼滤波(EnKF)原理及样例应用

    EnKF 是一种基于蒙特卡罗方法预测误差统计信息的卡尔曼滤波。它与 PF 的相同点是都采用了采样粒子的集合来表示状态概率空间,但 EnKF 在更新步使用卡尔曼更新,该方法以集合的形式进行模拟预报和 分析更新这两个过程,通过模式状态的集合来表征 误差协方差的信息,以最小

    2024年02月05日
    浏览(35)

觉得文章有用就打赏一下文章作者

支付宝扫一扫打赏

博客赞助

微信扫一扫打赏

请作者喝杯咖啡吧~博客赞助

支付宝扫一扫领取红包,优惠每天领

二维码1

领取红包

二维码2

领红包