随便写点关于卡尔曼滤波

看了一篇点击打开链接博客，说说我的理解

假设系统此时状态与上一时刻的状态有关系（差分方程）：

其中，xk为系统现在的状态变量，xk-1为系统上一时刻的状态变量，uk-1为系统上一时刻的输入，wk-1为噪声。由这个式子可以得到系统状态的预测值，

观测方程：

zk为观测值，xk为系统状态变量，vk为观测噪声。用这个式子和差分方程可以得到观测值的预测。

很自然地想到用反馈的方式。

重点就是求增益矩阵K。

考虑估计值与真实值的协方差矩阵，由于卡尔曼滤波考虑的误差是白噪声，因此协方差矩阵为一个对角线矩阵

把它展开最后得到

使误差最小，即是使协方差矩阵的迹最小

对其求导，令导数为0，得到K

把K化简后带回（这里没看懂，高博书上241页似乎有更靠谱解释），

式子中还有个未知，它是预测值与真实值的协方差矩阵：

至此，卡尔曼滤波的五条公式就出来了

1计算预测值：

2计算预测值与真实值协方差矩阵

3计算卡尔曼增益系数矩阵

4计算估计值

5计算估计值与真是值之间的协方差矩阵为下一次递推做准备

这里面的R与Q分别是状态方程和观测方程的噪声的方差（如何求？）。

考虑到SLAM中的运动方程和观测方程是

其中xk为机器人位姿,u是传感器读数（也可以认为是输入），y为路标，z为观测数据，w与v是噪声，显然，式子中的函数f与h不是一个线性关系，它们无法用矩阵的形式表达出来。对于运动方程，考虑其对xk-1求导后进行泰勒展开。

，把偏导部分记为F。

同理，对于观测方程

,把偏导部分记为H。

这样，扩展卡尔曼的预测部分和以前类似，但是估计与真实值的协方差矩阵变成了用偏导矩阵来求：

注意，这里之所以能照搬卡尔曼滤波的公式的形式实际上是因为在卡尔曼滤波中，我们估计值与真实值的误差表示形式是X-=A(X-)，而在扩展卡尔曼滤波中将f函数求偏导以后，估计值与真实值的误差表示形式是X-=F(X-)。下面计算卡尔曼增益的时候原理也类似。

卡尔曼增益矩阵的求法

获得估计值与下一次预测的预测值与真实值的协方差矩阵