无迹卡尔曼UKF详细公式推导

设X为被估计随机向量，对X的量测为随机向量Z，X基于Z的线性最小方差估计为：

其中：

估计的均方误差阵为：

当上述X和Z都服从正态分布时，所得到的估计就是最小方差估计。

设系统方程和量测方程具有离散形式

                    1- 1

                     1- 2

式中，f和h为非线性向量函数，和为不想过的零均值白噪声序列，方差阵为和，u（k-1）为确定性控制项。则基于的线性最小方差估计为：

        1- 3

其中,指的是给定条件变量Z从0到k下随机变量X的条件期望。

根据线性最小方差估计的线性性质，可得：

令，可得：

由于与互不相关，则：

由上式推导可得：

           1- 4

令，根据最小方差估计公式可得：

1- 5

上式中，均值可近似取为条件均值,

记

                                  1- 6

则

                                    1- 7

将1- 6式子带入1- 4可得：

                                            1- 8

同理，用条件均值近似均值，即,可估计的均方误差阵：

                                    1- 9

考虑到为对称阵，,根据上述式子可推：

                    1- 10

上述(1-6), (1-8),(1-10)构成了整个无迹卡尔曼滤波的核心，要进一步确定出增益阵Kk，并使算法具有递推性，还需用UT变换确定出及一步预测值

计算卡尔曼增益

系统状态更新

协方差更新

完整版已经在主页更新