UKF-SLAM是一种比较流行SLAM方案。相比EKF-SLAM,UKF利用unscented transform代替了EKF的线性化趋近,因而具有更高的精度。Bayes++库中的unsFlt.hpp中给出了UKF实现的相关类。

namespace Bayesian_filter
{ class Unscented_predict_model : public Predict_model_base
/* Specific Unscented prediction model for Additive noise
43 * x(k|k-1) = f(x(k-1|k-1)) + w(x(k))
44 *
45 * Unscented filter requires
46 * f the function part of the non-linear model
47 * Q the covariance of the additive w(x(k)), w is specifically allow to be a function of state
48 */
{
public:
Unscented_predict_model (std::size_t q_size)
{
q_unscented = q_size;
} virtual const FM::Vec& f(const FM::Vec& x) const = ;
// Functional part of additive model
// Note: Reference return value as a speed optimisation, MUST be copied by caller. virtual const FM::SymMatrix& Q(const FM::Vec& x) const = ;
// Covariance of additive noise
// Note: Reference return value as a speed optimisation, MUST be copied by caller.
private:
friend class Unscented_filter; // Filter implementation need to know noise size
std::size_t q_unscented;
}; class Unscented_scheme : public Linrz_kalman_filter, public Functional_filter
{
private:
std::size_t q_max; // Maximum size allocated for noise model, constructed before XX
public:
FM::ColMatrix XX; // Unscented form of state, with associated Kappa
Float kappa; Unscented_scheme (std::size_t x_size, std::size_t z_initialsize = );
Unscented_scheme& operator= (const Unscented_scheme&);
// Optimise copy assignment to only copy filter state void init ();
void init_XX ();
void update ();
void update_XX (Float kappa); void predict (Unscented_predict_model& f);
// Efficient Unscented prediction
void predict (Functional_predict_model& f);
void predict (Additive_predict_model& f);
Float predict (Linrz_predict_model& f)
{ // Adapt to use the more general additive model
predict(static_cast<Additive_predict_model&>(f));
return .; // Always well condition for additive predict
} Float observe (Uncorrelated_additive_observe_model& h, const FM::Vec& z);
Float observe (Correlated_additive_observe_model& h, const FM::Vec& z);
// Unscented filter implements general additive observe models Float observe (Linrz_uncorrelated_observe_model& h, const FM::Vec& z)
{ // Adapt to use the more general additive model
return observe (static_cast<Uncorrelated_additive_observe_model&>(h),z);
}
Float observe (Linrz_correlated_observe_model& h, const FM::Vec& z)
{ // Adapt to use the more general additive model
return observe (static_cast<Correlated_additive_observe_model&>(h),z);
} public: // Exposed Numerical Results
FM::Vec s; // Innovation
FM::SymMatrix S, SI; // Innovation Covariance and Inverse protected:
virtual Float predict_Kappa (std::size_t size) const;
virtual Float observe_Kappa (std::size_t size) const;
/* Unscented Kappa values
117 default uses the rule which minimise mean squared error of 4th order term
118 */ protected: // allow fast operation if z_size remains constant
std::size_t last_z_size;
void observe_size (std::size_t z_size); private:
void unscented (FM::ColMatrix& XX, const FM::Vec& x, const FM::SymMatrix& X, Float scale);
/* Determine Unscented points for a distribution */
std::size_t x_size;
std::size_t XX_size; // 2*x_size+1 protected: // Permanently allocated temps
FM::ColMatrix fXX;
}; }//namespace
#endif

Bayes++ Library入门学习之熟悉UKF相关类的更多相关文章

  1. Bayes++ Library入门学习之熟悉namespace

    Bayes++是一个开源的C++类库.这些类表示并实现了用于离散系统的贝叶斯滤波的各种数值算法.该库中的类提供测试和一致的数值方法,并且用层次明确的结构表明了各种滤波算法和系统模型类型. 接下来,我们 ...

  2. Bayes++ Library入门学习之熟悉class-Bayesian_filter_base(2)

    前面我们已经熟悉了Bayesian_filter::Bayes_filter_base和其子类的击继承关系,接下来我们开始学习该类的实现. bayesFlt.hpp文件为其实现主体,首先是两个常规的头 ...

  3. Bayes++ Library入门学习之熟悉class-Bayesian_filter_base(1)

    在对Bayes++库的名称空间有了一个大概的了解之后,我们开始学习该名称空间下的第一个子类Bayesian_filter::Bayes_filter_base. 该类与其子类的继承关系图如下图所示. ...

  4. Bayes++ Library入门学习之熟悉class-Importance_resampler

    接下来,需要介绍的是重要性重采样类Bayesian_filter::Improtance_resampler.该类实现了两种重采样方法[1][2],和其子类的继承关系图如下: 其中Standard_r ...

  5. 零基础入门学习Python(36)--类和对象:给大家介绍对象

    知识点 Python3 面向对象 Python从设计之初就已经是一门面向对象的语言,正因为如此,在Python中创建一个类和对象是很容易的.本章节我们将详细介绍Python的面向对象编程. 如果你以前 ...

  6. 第15.10节 PyQt(Python+Qt)入门学习:Qt Designer可视化设计界面组件与QWidget类相关的组件属性详解

    PyQt学习有阵子了,对章节的骨架基本考虑好了,准备本节就写组件的属性的,结果一是日常工作繁忙,经常晚上还要加班,二是Qt的组件属性很多,只能逐一学习.研究和整理,花的时间有点长,不过终于将可视化设计 ...

  7. OpenCV入门学习笔记

    OpenCV入门学习笔记 参照OpenCV中文论坛相关文档(http://www.opencv.org.cn/) 一.简介 OpenCV(Open Source Computer Vision),开源 ...

  8. SCARA——OpenGL入门学习一、二

    参考博客:http://www.cppblog.com/doing5552/archive/2009/01/08/71532.html 简介 最近开始一个机械手臂的安装调试,平面关节型机器人又称SCA ...

  9. opengl入门学习

    OpenGL入门学习 说起编程作图,大概还有很多人想起TC的#include <graphics.h>吧? 但是各位是否想过,那些画面绚丽的PC游戏是如何编写出来的?就靠TC那可怜的640 ...

随机推荐

  1. IE input X 去掉文本框的叉叉和password输入框的眼睛图标

    IE input X 去掉文本框的叉叉和password输入框的眼睛图标 从IE 10開始,type="text" 的 input 在用户输入内容后.会自己主动产生一个小叉叉(X) ...

  2. javaScript中的事件对象event

    事件对象event,每当一个事件被触发的时候,就会随之产恒一个事件对象event,该对象中主要包括了关于该事件的基本属性,事件类型type(click.dbclick等值).目标元素target(我的 ...

  3. hpc-ai比赛相关资料

    http://follitude.com/blog/Use-RDMA-Emulation-in-Software-Using-SoftiWARP/ https://www.reflectionsoft ...

  4. PIC c语言

    rom类型,对于占内存的类型定义为rom类型,跟标准c中的const不一样,const跟rom不能通用,否则编译会报type qualifier dismatch 有些变量定义成了rom型,那么如果改 ...

  5. Windows 10用户可以快速移除U盘

    新浪科技讯,北京时间 4 月 9 日上午消息,据美国科技媒体 The Verge 报道,微软再次证实,从 1809 版本的 Windows 10 开始,插入新闪存盘时“快速移除”(quick remo ...

  6. vue中的methods、computed和watch

    1.computed属性: 经过处理返回的数据值,只要源数据没有发生改变,computed函数里面对相应的数据就不会反生改变,相当于缓存在本地;发生改变的时候,computed对应数据的函数才会发生改 ...

  7. 紫书 习题 8-24 UVa 10366 (构造法)

    又是一道非常复杂的构造法-- #include<cstdio> #include<algorithm> #define REP(i, a, b) for(int i = (a) ...

  8. iOS基础UI控件介绍-Swift版

    iOS基础UI控件总结 iOS基础控件包括以下几类: 1.继承自NSObject:(暂列为控件) UIColor //颜色 UIImage //图像 2.继承自UIView: 只能相应手势UIGest ...

  9. HDU 4069 数独

    好久没做题了,建图搞了好久…… 然后,判是否有多解的时候会把原来的答案覆盖掉…… 这里没注意,弄了一下午…… 代码: #include <iostream> #include <cs ...

  10. [terry笔记]11gR2_DataGuard搭建_拷贝数据文件

    11gR2搭建dataguard环境: 自己做的实验,后续按照rman模式搭建.主备切换.模式调整等实验会陆续发上来. primary: OS:oel 6.4 database:11.2.0.4.0 ...