从贝叶斯滤波理论到容积卡尔曼滤波算法（CKF）详细推导及编程实现常转弯率模型估计。（matlab）

容积卡尔曼滤波（CKF）是由加拿大学者Arasaratnam和Haykin在2009年提出的。该算法的核心思想是针对非线性高斯系统，通过三阶球面径向容积准则来近似状态的后验均值和协方差，以保证在理论上以三阶多项式逼近任何非线性高斯状态的后验均值和方差。

• 贝叶斯滤波
• 球面-径向容积规则
• 容积卡尔曼滤波算法
• matlab实现"Cubature Kalman Filters"论文中常转弯率例子

贝叶斯滤波：

由于容积卡尔曼滤波是基于贝叶斯滤波理论框架的次优滤波，所以我们先来了解一下贝叶斯滤波。

在解决滤波问题时，如果我们能求出状态的后验概率密度函数，就可以通过状态的概率密度函数描述非线性系统的统计特性，再通过各种估计准则和近似方法就能得到不同的滤波算法。
系统状态后验概率密度决定了系统的多种统计特性。因此如何求得系统状态的后验概率密度函数成为关键。而贝叶斯估计的优点是给出了状态后验概率密度函数的递推求解方法，成为众多滤波算法的核心思想。

上面两个式子就组成了贝叶斯递推公式，我们可以看出贝叶斯滤波从理论上实现了非线性滤波的最优估计。

下图为贝叶斯滤波框图。

球面-径向容积规则：

由上一部分的贝叶斯滤波，我们很难直接求解上述积分。我们将上述积分变形为：

n维球坐标变换

由n维球坐标变换定理可将上面积分转换成球面径向积分：


将上面积分分解成球面积分和径向积分：


上述球面积分和径向积分都是通过容积规则来实现，容积规则是将一系列点集D用非线性函数进行传递，然后根据加权求和来近似非线性函数的后验均值和方差。
*

点集D有如下定义：

所以球面积分就变成了：




所以容积点可选取单位球的球面与其坐标系相交的点。
所以球面积分可近似为：

径向积分：

将球面积分的近似带入到径向积分中可得：


上面式子就是三阶球面径向容积规则的近似规则。
接下来我们将容积规则与高斯分布的积分相结合：



上面式子就是容积卡尔曼滤波使用的三阶球面径向容积规则。

接下来我们来看一下CKF算法的流程：

常转弯率模型CKF滤波估计（matlab）:

程序如下：

CKF函数：

function [Pk,Xjian]=CKF(Pk,n,Xjian,y,Q,R)
m = 2*n;  %n为状态维数
T=1;%时间
kesai=sqrt(m/2) * [eye(n),-eye(n)];
[S1]=chol(Pk)';%分解成AT*A（下三角*上三角）取下三角
Xr=S1*kesai+repmat(Xjian,1,m);%计算容积点,repmat()把元素复制m行n列
for i=1:m
    omiga = Xr(5,i);
    fai= [1,     sin(omiga*T)/omiga, 0, -(1-cos(omiga*T))/omiga, 0;