CKF算法实现
#CKF算法实现
1.容积规则实现复杂积分
注： **The points and weights ** of the cubature rule are independent with the integrand f ( x ) f(x) f(x).
所以，容积规则的点和权重可以脱机计算并预先存储以加快计算速度。
1.1 确定容积点和权重
Consider a multi-dimension weighted integral of the form
I ( f ) = ∫ D f ( x ) ω ( x ) d x I(f)=\int_Df(x)\omega(x)dx I(f)=∫Df(x)ω(x)dx
The basic task of numerically computing I ( f ) I(f) I(f) is to find a set of points x i x_i xi and weights ω i \omega_i ωi that approximates I ( f ) I(f) I(f) by a weighted sum of function evalutions .
I ( f ) ≈ ∑ i = 1 m ω i f ( x i ) I(f)\approx \sum^m_{i=1}\omega_if(x_i) I(f)≈i=1∑mωif(xi)
变量 x x x是服从高斯分布的， f f f是任意非线性函数。
An efficient non-product third-degree fully symmetric cubature rule is proposed to find { x i , ω i } \{x_i,\omega_i\} {xi,ωi} for **Gaussian weighted integrals. **
∫ R n f ( x ) N ( x ; μ , Σ ) d x = 1 π n ∫ R n f ( 2 Σ x + μ ) e − x T x d x \int_{R^n}f(x)N(x;\mu,\Sigma)dx = \frac{1}{\sqrt{\pi^n}} \int_{R^n}f(\sqrt{2\Sigma}x+\mu)e^{-x^Tx}dx ∫Rnf(x)N(x;μ,Σ)dx=πn1∫Rnf(2Σx+μ)e−xTxdx
Combine the spherical rule and the radial rule
∫ R n f ( x ) e − x T x d x ≈ ∑ j = 1 m s ∑ i = 1 m r a i b j f ( r i s j ) \int_{R^n}f(x)e^{-x^Tx}dx \approx \sum^{m_s}_{j=1} \sum^{m_r}_{i=1}a_ib_jf(r_is_j) ∫Rnf(x)e−xTxdx≈j=1∑msi=1∑mraibjf(risj)
对于一个标准高斯加权积分有：
（当n一定， a i = Γ ( n / 2 ) 2 , b j = 2 π n 2 n Γ ( n / 2 ) , r i = n / 2 , s j = 1 a_i=\frac{\Gamma(n/2)}{2},b_j=\frac{2\sqrt{\pi^n}}{2n\Gamma(n/2)},r_i=\sqrt{n/2},s_j=1 ai=2Γ(n/2),bj=2nΓ(n/2)2πn,ri=n/2,sj=1都是常值；且 m r = 1 m_r=1 mr=1，一个点； m = m s = 2 n m=m_s=2n m=ms=2n）
I N ( f ) = ∫ R n f ( x ) N ( x ; 0 , I ) d x = 1 π n ∫ R n f ( 2 x ) e − x T x d x ≈ 1 π n ∑ j = 1 m Γ ( n / 2 ) 2 b j f ( 2 n / 2 s j ) = ∑ i = 1 m 1 2 n f ( 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^Tx}dx \approx \frac{1}{\sqrt{\pi^n}}\sum^{m}_{j=1} \frac{\Gamma(n/2)}{2} b_jf(\sqrt{2}\sqrt{n/2}s_j)=\sum^m_{i=1}\frac{1}{2n}f(\sqrt{n}[1]_i) IN(f)=∫Rnf(x)N(x;0,I)dx=πn1∫Rnf(2x)e−xTxdx≈πn1j=1∑m2Γ(n/2)bjf(2n/2sj)=i=1∑m2n1f(n[1]i)
For Gaussian distribution with non-zero mean and non-unity covariance, the cubature points will be located at ( Σ [ 1 ] i + μ ) ( \sqrt{ \Sigma }[1]_i+\mu) (Σ[1]i+μ).[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