离散卡尔曼滤波PPT课件.ppt

卡尔曼滤波 TheoryofKalmanfilter 1 1卡尔曼滤波与最优估计 卡尔曼滤波是一种最优估计技术 它能将仅与部分状态有关的测量值进行处理 得出从某种统计意义上讲估计误差最小的更多的状态的估计值 估计误差最小的标准称为估计准则 根据不同的估计准则和估计计算方法 有各种不同的最优估计 卡尔曼滤波是一种递推线性最小方差估计 2 1 1最小方差估计 最小方差估计的估计准则是估计的均方误差最小 即 系统的n维随机向量 Z是m维随机量测向量 利用Z计算得到的X的最小方差估值 估计的误差 估计均方差阵 最小方差估计的误差小于等于其他估计的均方误差 3 估计的均方误差就是估计误差的方差 即 最小方差估计具有无偏性质 即它的估计误差 亦可用表示 的均值为零 即 因此 最小方差估计不但使估值的均方误差最小 而且这种最小的均方误差就是估计的误差方差 4 1 2线性最小方差估计 如果将估值规定为量测矢量Z的线性函数 即 使得下述指标满足式中A和b分别是 n m 阶和n维的矩阵和矢量 这样的估计方法称为线性最小方差估计 有时用符号E X Z 表示 5 有关量测量Z的线性函数有无穷多个 但能使X具有最小方差估计的线性函数只有一个 记为 利用上述的指标我们可以得出A0和b0 6 因此X在Z上的线性最小方差估计为 线性最小方差估计的均方误差为 7 1 2 1线性最小方差估计的性质 性质1无偏性线性最小方差估计是X在Z上的无偏估计 即 性质2线性1线性最小方差估计是具有线性性质 即若X的线性最小方差估计为 则的线性最小方差估计为 8 其中F为确定性矩阵 e为确定性向量 性质3线性2若Y与Z不相关 则 9 1 3递推线性最小方差估计 卡尔曼滤波 卡尔曼滤波的准则与线性最小方差估计相同 估值同样是量测值的线性函数 只要包括初始值在内的滤波器初值选择正确 它的估值也是无偏的 计算方法 递推形式 10 在k时刻以前估值的基础上 根据k时刻的量测值Zk 递推得到k时刻的状态估计值 根据k 1时刻以前所有的量测值得到 X k 也可以说是综合利用k时刻以前的所有量测值得到的 一次仅处理一个量测量计算量大大减小 主要适用于线性动态系统 11 2卡尔曼滤波方程 2 1离散系统的数学描述 设离散化后的系统状态方程和量测方程分别为 Xk为k时刻的n维状态向量 被估计量 Zk为k时刻的m维量测向量 k 1到k时刻的系统一步状态转移矩阵 n n阶 Wk 1为k 1时刻的系统噪声 r维 k 1为系统噪声矩阵 n r阶 Hk为k时刻系统量测矩阵 m n阶 Vk为k时刻m维量测噪声 12 Qk和Rk分别称为系统噪声和量测噪声的方差矩阵 在卡尔曼滤波中要求它们分别是已知值的非负定阵和正定阵 kj是Kronecker 函数 即 卡尔曼滤波要求 Wk 和 Vk 是互不相关的零均值的白噪声序列 有 13 Var 为对 求方差的符号 卡尔曼滤波要求mx0和Cx0为已知量 初始状态的一 二阶统计特性为 且要求X0与 Wk 和 Vk 都不相关 14 2 2离散卡尔曼滤波方程 或 15 2 2离散卡尔曼滤波方程 时间修正方程 量测修正方程 16 1 状态一步预测方程 各滤波方程的物理意义 Xk 1的卡尔曼滤波估值 利用Xk 1计算得到的一步预测 也可以认为是利用k 1时刻和以前时刻的量测值得到的Xk的一步预测 17 上式就是通过计算新息 把估计出来 并左乘一个系数矩阵加到中 从而得到估值和 称为滤波增益矩阵 2 状态估值计算方程 计算估值Xk的方程 它是在一步预测Xk k 1的基础上 根据量测值Zk计算出来的 一步预测误差 若把看作是量测的一步预测 则就是量测的一步预测误差 由两部分组成 和 正是在基础上估计所需信息 因此又称为新息 18 由于也具有无偏性 即的均值为零 所以也称为一步预测误差方差阵 上式中的和分别就是新息中的两部分内容 3 滤波增益方程 一步预测均方差阵 即 Kk选取的标准就是卡尔曼滤波的估计准则 也就是使得均方误差阵最小 如果Rk大 Kk就小Rk小 Kk就大 19 4 一步预测均方误差方程 从下式可以看出 求Kk必须先求出Pk k 1 式中 为的估计误差 可以看出一步预测均方误差阵Pk k 1是从估计均方误差阵Pk 1转移过来的 并且再加上系统噪声方差的影响 的均方误差阵 即 20 2020 2 6 21 5 估计均方误差方程 或 计算量小 但在计算机有舍入误差的条件下 不能始终保证算出的Pk是对称的 22 6 卡尔曼滤波的计算流程 滤波计算回路 增益计算回路 23 2 3离散卡尔曼滤波基本方程使用要点 在滤波开始时 必须有初始值和才能进行 为了保证估值的无偏性 应选择 这样才能保证估计均方差阵Pk始终最小 1 滤波初值的选取 24 有上述的卡尔曼滤波基本方程中的均方误差的公式 2 估计均方误差阵的等价形式及选用 a b c 25 由卡尔曼滤波方程的推导得知 基本方程只适用于系统方程和量测方程都是离散型的情况 但实际的物理系统一般都是连续的 动力学特性用连续的微分方程来描述 所以在使用卡尔曼基本方程之前 必须对系统方程和量测方程进行离散化处理 设描述物理系统动力特性的系统方程为 3 一步转移阵和等效离散系统噪声方差阵的计算 其中系统驱动源W t 为白噪声过程 即 26 q为w t 的方差强度矩阵 根据线性系统理论 系统方程的离散化形式为 其中 满足方程 27 对该方程求解并进行约等变化 得 式中T为滤波周期 这个式子即为一步转移矩阵的实时计算公式 同样 通过系统的离散化处理 得出等效离散系统噪声方差阵 28 一步预测方程改为 状态估计方程改为 其他滤波方程不变 4 系统有确定性控制时的滤波基本方程 设系统除了白噪声外 还有确定性驱动项 29 5 一步预测基本方程 令 一步预测基本方程式指利用递推计算的全套方程 根据基本方程的一步预测方程得 30 则 从而也可以得到均方误差 31 例1设有线定常系统 式中状态变量Xk和量测Zk均为标量 为常数 Wk和Vk为零均值白噪声序列 分别具有协方差 并且Wk Vk X0三者互不相关 求的递推方程 32 33 例2 滤波设运动体沿某一直线运动 tk时刻的位移 速度 加速度 加加速度分别为