卡尔曼滤波原理..doc

过程方程： X(k+1)?=??A?X(k) +?B?U(k) + W(k)?????????????? 式1 量测方程： Z(k+1)?=??H?X（k+1）+ V(k+1)????????????????? 式2 A和B是系统参数，对于多模型系统，他们为矩阵；H是测量系统的参数，对于多测量系统，H为矩阵。W(k)和V(k)分别表示过程和测量的噪声。他们被假设成高斯白噪声，他们的协方差 分别是Q，R。为了不失一般性，下面的讨论中将X,Z都视为矩阵，其中X是m行的单列矩阵，Z是n行的单列矩阵。 ? 说明：下面的表达式中，不带前缀的量都代表实际量，其小括号里面的“k”或“k+1”代表该量是第k或第k+1时刻的实际量，如“Z(k+1)”就代表第k+1时刻的实际测量值； 带前缀“^”的量都代表预测量，如果小括号里面是“k+1|k”，就代表k+1时刻的先验预测值，如果小括号里面是“k+1|k+1”，就代表k+1时刻的后验预测值；（测量值可以通过测量得到，所以只有先验预测，没有后验预测。而实际状态值无法得知，既有先验预测，又有后验预测） 带前缀“~”的量都代表与预测值对应的偏差值。 ? 实际状态值与先验预测状态值的偏差 = 实际状态值 – 先验预测状态值 ~X(k+1|k)????? =???? X(k+1)??? -????? ^X(k+1|k)????? ?????? 式3 ? 实际测量值与先验预测测量值的偏差 = 当前测量值 - 先验预测测量值 ~Z(k+1|k)? =?Z(k+1)? -?? ^Z(k+1|k)?????????????????????????????????? 式4 ? ? 并且 先验预测测量值? =? 转换矩阵H?* 先验预测状态值 ^Z(k+1|k)?=??H?^X(k+1|k) ?????????????????????????? 式5 ? 得到测量值后，再对当前状态值X(k+1) 进行后验预测（设后验预测值为 ^Z(k+1|k+1) ） ，则后验预测值（同时也是最终预测值）的偏差为 ~X(k+1|k+1)? =???? X(k+1)??? -????? ^X(k+1|k+1)?? ????????????? 式6 ? ? 为了得到当前状态值X(k+1)， 根据式3，需要： X(k+1)?=? ^X(k+1|k)? +?~X(k+1|k)? ????????????????????? 式7 上式中，我们可以通过卡尔曼公式1（见附注2）计算出^X(k+1|k)，但我们无法得知实际状态值X(k+1)，因而~X(k+1|k) 也无法得知。我们最终的目的是得出一个比较接近实际状态值X(k+1)的滤波值^X(k+1|k+1)，根据式7，只要能准确的估计出~X(k+1|k)即可。 ~X(k+1|k)本身虽无法得知，但~Z(k+1|k) 却可以通过测量得到，而且它们二者存在一定的相关性。不妨再设存在一个矩阵K（m行n列矩阵），能使得 ?~X(k+1|k) = K * ~Z(k+1|k) ?????????????????????????????????????? 式8 那么最终的预测任务其实就是找到K。由于~X(k+1|k)和~Z(k+1|k)都是单列矩阵，因此不难看出，满足式8的矩阵K应有无穷多个。矩阵K中第i行第j列反映了量测变量偏差矩阵~Z(k+1|k)的第j个元素对状态变量偏差矩阵~X(k+1|k)的第i个元素的贡献。因此矩阵K的物理意义很明显，K的第i行第j列的元素表示：对于第i个待测的状态量来说，第j个测量仪器测到的偏差的可信度。某个测量值对应的可信度越高，滤波器越“相信”该测量值。 ? 既然满足条件的K有无穷多个，那应该使用哪个K呢？实际上，我们并不知道~X(k+1|k)的值，所以也就无法直接计算出K，而只能通过某种方法找到一个Kg，使得将Kg带入式8后，等号两边的差（的平方）的期望尽可能小。 我们最终的预测值或滤波值是后验预测值^X(k+1|k+1)，因此最后的预测也应使 ~X(k+1|k+1) 的期望为0且方差最小（这与让8式两端的差最小是一致的，下面的式9体现了这一点），这样预测值才最可靠。下面详细说明。 ? ^X(k+1|k+1) = ?^X(k+1|k) +?Kg * ~Z(k+1|k)??????? （后验预测的状态值） ~X(k+1|k+1)? =???? X(k+1)??? -????? ^X(k+1|k+1)?? ?（后验预测的偏差） ? ~X(k+1|k+1)? =?????????????????? X(k+1)???????????????????????? -???????????? ^X(k+1|k+1)?? ???????????????????? =???? (?^X