卡尔曼滤波(四)(最优化数字处理算法)

一、先验误差的协方差矩阵求解


引入过程噪音,计算得到的状态变量:X(k)= A2 *  X(k-1)+ B2 * U(k)+ W(k-1)

引入测量噪音后,测量得到的状态变量:Z(k)= H * X(k)+ V(k)

对于X(k)的理解:

(1)X(k)是引入了不确定性后得到的状态变量,它弥补了由于模型不够准确而缺失的那部分影

响,它是我们想要的真实的结果,但是由于W(k-1)这部分无法得到,所以X(k)得不到

(2)如果在现实得到的Z(k)中剔除了V(k)成分,就可以得到X(k),它是我们想要的真实的

结果,但是由于V(k)这部分无法得到,所以X(k)得不到


先验估计值:X_Hat(k)= A2 * X_Hat_New(k-1)+ B2 * U(k)

先验误差:E(k)= X(k)-X_Hat(k) 

过程噪音的协方差矩阵:Q = E【W * W的转置】

先验误差可用它的协方差矩阵来量化表示:P【E(k)】 = E【E(k)* E(k)的转置】

卡尔曼增益系数:K(k) = ( H * P【K(k)】* H的转置 + R )的逆 * P【K(k)】* H的转置


这一章是为了求解先验误差的协方差矩阵P【K(k)】的表达式


而先验误差E(k) = X(k)-X_Hat(k)  ,X(k)和X_Hat(k)的表达式都知道,最后可以得到

先验误差E(k)的表达式:E(k) = A2 * E(k-1)+ W(k-1),然后把这个表达式代入到先验误

差的协方差矩阵的表达式当中,也就是P【E(k)】 = E【E(k)* E(k)的转置】,最后可得:

P【E(k)】= A2 * P_New【 E_New(k-1)】* A2的转置 + Q

二、使用卡尔曼滤波器的步骤


主要分为三个步骤:

(1)第一步是建立估计方程和测量方程(注意:由于传感器得到的值,已经包括测量噪音,所以

测量方程可以不用再加入测量噪音这一项;而估计方程在建立的时候,可以把外部的干扰引入到状

态变量这个向量里面,也可以不引入)

(2)第二步是预测

(3)第二步是校正

也就是使用上一次的最优结果(也就是后验估计值)预测这一次的值(也就是先验估计值),同时

使用测量值来修正这一次的预测值,来得到这一次的最优结果


对于预测,先求解先验估计值,再求解先验误差的协方差矩阵

先验估计值:X_Hat(k)= A2 * X_Hat_New(k-1)+ B2 * U(k)

先验误差的协方差矩阵:P【E(k)】= A2 * P_New【 E_New(k-1)】* A2的转置 + Q


对于校正,先求解卡尔曼增益系数,再求解后验估计值给下一次计算先验误差值做准备,最后更新

后验误差的协方差矩阵给下一次计算先验误差的协方差矩阵做准备

卡尔曼增益系数:K(k) = ( H * P【E(k)】* H的转置 + R )的逆 * P【E(k)】* H的转置

后验估计值:X_Hat_New(k) = X_Hat(k) + K(k)*【Z(k) - H * X_Hat(k)】

更新后验误差的协方差矩阵:P_New【 E_New(k)】=(单位阵I - K(k)* H)* P【E(k)】

(这个表达式在第三篇中末尾处有给出P_New【 E_New(k)】的由来,再把K(k)代入即可)卡尔曼滤波(四)(最优化数字处理算法)_第1张图片

最后发现,卡尔曼增益系数是跟Q矩阵和R矩阵有关的,对应于过程噪音和测量噪音的协方差矩阵

三、初始化参数的设置


调节Q矩阵和R矩阵,Q矩阵取决于模型建立与现实的差距,R矩阵取决于传感器测量的精度和现实

的干扰

设置第一次后验估计值和后验误差的协方差矩阵,这里可以随意设置,经过几次迭代计算之后就可

以趋于理论稳定值。习惯设置后验估计值都为0,后验误差的协方差矩阵往小的进行取值,一般为1

但不可以为0

你可能感兴趣的:(数字处理算法,算法,人工智能,python)