CKF算法实现

　

ckf

CKF算法实现

1.容积规则实现复杂积分

注： The points and weights of the cuBATure rule are independent with the integrand f(x)" role="presentation" style="position: relative;">f(x)$f(x)$.

所以，容积规则的点和权重可以脱机计算并预先存储以加快计算速度。

1.1 确定容积点和权重

Consider a multi-dimension weighted integral of the form

I(f)=∫Df(x)ω(x)dx" role="presentation">I(f)=∫Df(x)ω(x)dx$$I(f)={\int }_{D}f(x)\omega (x)dx$$

The basic task of numerically computing I(f)" role="presentation" style="position: relative;">I(f)$I(f)$ is to find a set of points xi" role="presentation" style="position: relative;">xi$x_i$ and weights ωi" role="presentation" style="position: relative;">ωi${\omega }_i$ that APProximates I(f)" role="presentation" style="position: relative;">I(f)$I(f)$ by a weighted sum of function evalutions .

I(f)≈∑i=1mωif(xi)" role="presentation">I(f)≈∑i=1mωif(xi)$$I(f)\approx \sum _{i=1}^m{\omega }_{i}f(x_i)$$

变量x" role="presentation" style="position: relative;">x$x$是服从高斯分布的，f" role="presentation" style="position: relative;">f$f$是任意非线性函数。

An efficient non-product third-degree fully symmetric cubature rule is proposed to find {xi,ωi}" role="presentation" style="position: relative;">{xi,ωi}$\{x_i,{\omega }_i\}$ for Gaussian weighted integrals.

∫Rnf(x)N(x;μ,Σ)dx=1πn∫Rnf(2Σx+μ)e−xTxdx" role="presentation">∫Rnf(x)N(x;μ,Σ)dx=1πn−−√∫Rnf(2Σ−−−√x+μ)e−xTxdx$${\int }_{R^n}f(x)N(x;\mu ,\mathrm{\Sigma })dx=\frac{1}{\sqrt{{\pi }^n}}{\int }_{R^n}f(\sqrt{2\mathrm{\Sigma }}x+\mu )e^{-x^{T}x}dx$$

Combine the spherical rule and the radial rule

∫Rnf(x)e−xTxdx≈∑j=1ms∑i=1mraibjf(risj)" role="presentation">∫Rnf(x)e−xTxdx≈∑j=1ms∑i=1mraibjf(risj)$${\int }_{R^n}f(x)e^{-x^{T}x}dx\approx \sum _{j=1}^{m_s}\sum _{i=1}^{m_r}{a}_i{b}_{j}f(r_i{s}_j)$$

对于一个标准高斯加权积分有：

（当n一定，ai=Γ(n/2)2,bj=2πn2nΓ(n/2),ri=n/2,sj=1" role="presentation" style="position: relative;">ai=Γ(n/2)2,bj=2πn√2nΓ(n/2),ri=n/2−−−√,sj=1$a_i=\frac{\mathrm{\Gamma }(n/2)}{2},b_j=\frac{2\sqrt{{\pi }^n}}{2n\mathrm{\Gamma }(n/2)},r_i=\sqrt{n/2},s_j=1$都是常值；且mr=1" role="presentation" style="position: relative;">mr=1$m_r=1$，一个点；m=ms=2n" role="presentation" style="position: relative;">m=ms=2n$m=m_s=2n$）

IN(f)=∫Rnf(x)N(x;0,I)dx=1πn∫Rnf(2x)e−xTxdx≈1πn∑j=1mΓ(n/2)2bjf(2n/2sj)=∑i=1m12nf(n[1]i)" role="presentation">IN(f)=∫Rnf(x)N(x;0,I)dx=1πn−−√∫Rnf(2–√x)e−xTxdx≈1πn−−√∑j=1mΓ(n/2)2bjf(2–√n/2−−−√sj)=∑i=1m12nf(n−−√[1]i)$$I_N(f)={\int }_{R^n}f(x)N(x;0,I)dx=\frac{1}{\sqrt{{\pi }^n}}{\int }_{R^n}f(\sqrt{2}x)e^{-x^{T}x}dx\approx \frac{1}{\sqrt{{\pi }^n}}\sum _{j=1}^m\frac{\mathrm{\Gamma }(n/2)}{2}{b}_{j}f(\sqrt{2}\sqrt{n/2}{s}_j)=\sum _{i=1}^m\frac{1}{2n}f(\sqrt{n}[1{]}_i)$$

For Gaussian distribution with non-zero mean and non-unity covariance, the cubature points will be located at (Σ[1]i+μ)" role="presentation" style="position: relative;">(Σ−−√[1]i+μ)$(\sqrt{\mathrm{\Sigma }}[1{]}_i+\mu )$.^[2]

1.2 编程实现

[1] Arasaratnam I, Haykin S. Cubature Kalman Filters[J]. IEEE transactions on Automatic Control, 2009, 54(6):1254-1269.

[2] Bhaumik, Shovan, Cubature quadrature Kalman filter, 2013

相关阅读

关于ckfinder+ckeditor 图片上传路径问题

本人java 小白。 在做公司的小项目用的是jeesite，里面的图片和富文本用的是 ckfinder+ckeditor。过程中遇到了路径的问题。百度了

OpenCV：计时函数getTickCount、getTickFrequency

1. getTickCount() & getTickFrequency() getTickCount()：返回CPU自某个时间以来走过的时钟周期数。getTickFrequency()：返回CPU

需要在Transactional注解指定rollbackFor或者在方法中

需要在Transactional注解指定rollbackFor或者在方法中显示的rollback 芥末无疆sss 关注2018.02.14 01:53 字数 842 阅读 39评

winsockfix无法修复winsock怎么办？

WinsockFix是一款超级好用的winsock和TCP/IP修复工具，平时在使用计算机时，难免会碰到不良的软件，其附带的病毒有可能会篡改电脑的注