第七讲状态估计-卡尔曼滤波.ppt

智能信息处理技术 第七讲状态估计 卡尔曼滤波 智能信息处理技术 2 状态估计的主要内容 应用 通过数学方法寻求与观测数据最佳拟合的状态向量 1 确定运动目标的当前位置与速度 2 确定运动目标的未来位置与速度 3 确定运动目标的固有特征或特征参数 智能信息处理技术 3 状态估计主要内容 位置与速度估计 位置估计 距离 方位和高度或仰角的估计 速度估计 速度 加速度估计 智能信息处理技术 4 状态估计的主要方法 1 滤波2 滤波3 卡尔曼滤波这些方法针对匀速或匀加速目标提出 如目标真实运动与采用的目标模型不一致 滤波器发散 智能信息处理技术 5 算法的改进及适应性 状态估计难点 机动目标的跟踪1 自适应 滤波和自适应Kalman滤波均改善对机动目标的跟踪能力 2 扩展Kalman滤波针对卡尔曼滤波在笛卡儿坐标系中才能使用的局限而提出 智能信息处理技术 6 卡尔曼滤波器 卡尔曼滤波器的应用 通信 雷达 导航 自动控制等领域 航天器的轨道计算 雷达目标跟踪 生产过程的自动控制等 智能信息处理技术 7 卡尔曼滤波器的应用特点 对机动目标跟踪中具有良好的性能 为最佳估计并能够进行递推计算 只需当前的一个测量值和前一个采样周期的预测值就能进行状态估计 智能信息处理技术 8 卡尔曼滤波器的局限性 卡尔曼滤波器解决运动目标或实体的状态估计问题时 动态方程和测量方程均为线性 智能信息处理技术 9 一 数字滤波器作估值器 1 非递归估值器2 递归估值器 智能信息处理技术 10 1 非递归估值器 采样平均估值器 采用时域分析方法在掺杂有噪声的测量信号中估计信号x 智能信息处理技术 11 根据数字信号处理我们知道 所谓非递归数字滤波器是一种只有前馈而没有反馈的滤波器 它的冲击脉冲响应是有限的 在许多领域有着广泛的应用 假定用zk表示观测值 zk x nk 式中 x 恒定信号或称被估参量 nk 观测噪声采样 假定 E x x0 D x 2x E nk 0 E n2k 2n 智能信息处理技术 12 h1 h2 hm是滤波器的脉冲响应hj的采样 或称滤波器的加权系数 滤波器的输出 当h1 h2 hm 1 m时 该式表明 估计是用m个采样值的平均值作为被估参量x的近似值的 故称其为采样平均估值器 智能信息处理技术 13 估计的均方误差以P 表示 有 当i j时 ij 1 当i j时 ij 0 有 最后得 智能信息处理技术 14 结论 估计值是用m个采样值的平均值作为被估参量x的近似值 估值器的均方误差随着m的增加而减少 该估值器是一个无偏估值器 智能信息处理技术 15 2 递归估值器 一阶递归估值器 a为滤波器的加权系数 a 1 智能信息处理技术 16 递归数字滤波器是一种带有反馈的滤波器 它有无限的脉冲响应 有阶数少的优点 但其暂态过程较长 关于信号和噪声的基本假设与非递归情况相同 上图给出的一阶递归滤波器输入输出信号关系如下 式中 zk与非递归情况相同 a是一个小于1的滤波器加权系数 如果它大于或等于1 该滤波器就不稳定了 智能信息处理技术 17 k时刻的输出 yk ak 1z1 ak 2z2 azk 1 zk 将zk中的信号和噪声分开 并代入 有输出 由于 a 1 故随着k值的增加 yk趋近于x 1 a 这样 如果以 1 a yk作为x的估计值 则 智能信息处理技术 18 此时信号x和估值之间只差一个噪声项 当k值较大时 估值的均方误差 而一次取样的均方误差 故上一结果的均方误差约为一次采样的 1 a 1 a 倍 智能信息处理技术 19 二 线性均方估计 1 最优非递归估计 标量维纳滤波 2 递归估计 智能信息处理技术 20 1 最优非递归估计 非递归滤波器的估计值及其估计误差可分别表示为 智能信息处理技术 21 对m个参数逐一求导 令等于零 在均值为零的白噪声的情况下 可得到最小均方误差和估计 其中 b 2n 2x 在b m时 这种估计近似于采样平均 在噪声方差 2n较大时 其性能明显优于非最佳情况 这种最小均方误差准则下的线性滤波 通常称作标量维纳滤波 hj与非最优情况的不同 这里的滤波器的加权系数为 智能信息处理技术 22 2 由最优非递推估计导出递归估计 由前可知 非递归估值器可以表示为 条件与前面相同 对k 1次取样 相应的估计量 相应的估计误差 智能信息处理技术 23 由b 2n 2x及hi k 1 k b 有 所以有 智能信息处理技术 24 于是 分成二项 将第一项同时乘 除一个bk 则 智能信息处理技术 25 或 最后有 智能信息处理技术 26 最优递归估计器 递推公式 智能信息处理技术 27 最优递归估计器 递推公式 智能信息处理技术 28 递推开始时的初始条件应满足 以使为最佳值 解之 得 这时的如果E x 0 可从零开始递推运算 即 智能信息处理技术 29 三 标量卡尔曼滤波器 时变信号 主要作用 对掺杂有噪声的随机信号进行线性估计 智能信息处理技术 30 1 模型 1 信号模型 设要估计的随机信号为由均值为0 方差为 2w的白噪声激励的一个一阶递归过程 即信号对时间变化满足动态方程 x k ax k 1 w k 1 式中 a 系统参数 w k 1 白噪声采样 如果令x 0 0 E w k 0 则 智能信息处理技术 31 该过程称作一阶自回归过程 x k 的均值和方差分别为 自相关函数 智能信息处理技术 32 2 观测模型 观测模型由下式给出 z k cx k v k 式中 c 测量因子 v k E 0 D 2n的白噪声 最优递推估值器的信号和观测模型如图所示 智能信息处理技术 33 最优递推估值器的信号和观测模型 智能信息处理技术 34 2 标量卡尔曼滤波器 由前将递归估计的形式写成 均方误差 分别对a k 和b k 求导 并令其等于0 求其最佳估计 得出a k 与b k 的关系 a k a 1 cb k 最后有递归估值器 智能信息处理技术 35 b k 为滤波器增益 其中 均方误差 对于给定的信号模型和观测模型 上述一组方程便称为一维标量卡尔曼滤波器 其结构如图所示 智能信息处理技术 36 标量卡尔曼滤波器结构 智能信息处理技术 37 3 标量卡尔曼预测器 标量卡尔曼滤波是对掺杂有噪声的随机信号进行线性估计 但经常要对信号的未来值进行预测 特别是在控制系统中 根据预测提前时间的多少 把预测分成1步 2步 m步预测 通常把1步预测记作 预测的步数越多 误差越大 这里讨论1步预测问题 信号模型和观测模型同前 智能信息处理技术 38 根据前一节 有一步线性预测递推公式 其中 a k 和 k 可以通过使均方预测误差最小来确定 预测的均方误差可表示为 将预测方程代入该式 并求导 就会得到一组正交方程 智能信息处理技术 39 解之 得 a k a c k 将其代入预测方程 有 进一步可求出 其中 由以上表达式可以看出 可根据均方预测误差P k k 1 计算 k 然后再给出P k 1 k 的均方预测误差 智能信息处理技术 40 最优一步预测器 智能信息处理技术 41 最优一步预测及滤波器 智能信息处理技术 42 四 向量卡尔曼滤波器 1 信号向量和数据向量 如果要求对q个信号进行同时估计 这q个信号在k时刻的采样值记作x1 k x2 k xq k 假设每个信号都是由一阶自回归过程产生的 即第 个信号在时刻k的采样值为 x k a x k 1 w k 1 1 2 q 每个w 过程都是白的 零均值的 与其它过程的采样是独立的 于是把q个信号与q个白噪声组成的q维向量分别表示成 智能信息处理技术 43 显然 X k AX k 1 W k 1 式中 X k X k 1 W k 1 都是q维向量 A是个q q阶矩阵 即 如果信号不满足一阶递归差分方程 而满足二阶递归差分方程 即 x k ax k 1 bx k 2 w k 1 智能信息处理技术 44 定义两个分量 x1 k x k x2 k x1 k 1 x k 1 于是 有 最后 有 X k AX k 1 W k 1 结果把一个二阶差分方程变成了一个一阶二维向量方程 该方程用起来更简单方便 智能信息处理技术 45 写成一般形式 其中 智能信息处理技术 46 写成向量形式 最后 有 即可写成一阶向量的形式 在对信号向量进行估计的过程中 同时产生r个含有噪声的测量值 记作z1 k z2 k zr k 则得到一组观测方程 智能信息处理技术 47 其中 vi k 表示附加噪声 ci表示第i个测量参数 于是有 Z k CX k V k 式中 Z k V k 是r维向量 X k 是q维向量 C是r q阶矩阵 对于r q 有 C即是观测矩阵 智能信息处理技术 48 2 向量问题的表示 根据前面的讨论 我们完全可以把前面的信号模型动态方程和观测方程写成如下形式 采用标量运算和矩阵运算的等价关系 推广到多维情况 智能信息处理技术 49 据此 可以将观测噪声的方差变成协方差矩阵 对两个信号的情况 则有 同理 也可以把系统噪声的方差变成协方差矩阵 即 由于系统噪声采样互不相关 该协方差矩阵的非对角线元素的值均为零 单一信号均方误差也可变成协方差矩阵 智能信息处理技术 50 3 向量卡尔曼滤波器 利用前面的概念 直接把标量卡尔曼滤波器公式变成向量卡尔曼滤波器公式 滤波器增益 式中 实际上 它是预测协方差 误差协方差矩阵 智能信息处理技术 51 用K k 代替了B k 因K k 是通用符号 如图 向量卡尔曼滤波器结构 智能信息处理技术 52 增益矩阵K k 的计算流程如图所示 增益矩阵计算流程 智能信息处理技术 53 4 向量卡尔曼预测器 根据相同的推导方法 可以获得卡尔曼预测器方程组 预测方程 预测增益 预测均方误差 它们与标量的情况是一一对应的 只是用G k 代替了 k 就可以将滤波和预测用同一个方框图表示出来 智能信息处理技术 54 5 总结 卡尔曼滤波器应用广泛 这里只对其进行简单归纳 1 卡尔曼滤波器的主要特性 卡尔曼滤波器是一个递归 线性 无偏和方差最小的滤波器 如果过程噪声和观测噪声是正态高斯白噪声 则它保持最佳特性 智能信息处理技术 55 2 卡尔曼滤波器模型 目标运动模型 位置测量模型 智能信息处理技术 56 状态方程 X t T t X t W t Q t E W t W t T 观测方程 Z t HX t V t R t E V t V t T 智能信息处理技术 57 3 卡尔曼滤波器方程组 残差 预测方程 状态估计 卡尔曼滤波器增益 智能信息处理技术 58 预测协方差 估计协方差 智能信息处理技术 59 五 卡尔曼滤波器的应用 1 系统矩阵 假定系统矩阵是四维矩阵 即距离 速度 方位角及其变化率 它们分别由R 和表示 距离方向上的加速度和角度方向的加速度分别由ur k 和u k 表示 状态方程为 智能信息处理技术 60 则系统方程为 智能信息处理技术 61 2 观测矩阵 假定观测值只有距离和方位两个 即R和 分别用z1和z2来表示 它们是由状态值和测量噪声组成的 且测量噪声是相互独立的零均值的白噪声 测量方程 则有 智能信息处理技术 62 其中 x1 k r k x3 k k 以上两个问题实际上是建立模型问题 智能信息处理技术 63 3 观测噪声协方差矩阵 在计算滤波器增益时 需知观测噪声的协方差矩阵 由于只有两个参数 因此 这里利用了方位和距离观测噪声相互独立的条件 故左下角和右上角项为零 智能信息处理技术 64 4 系统噪声协方差矩阵 假定目标作匀速运动 由于大气湍流等因素的影响 目标产生随机加速度 在距离和方位上都存在随机扰动 于是有 且 智能信息处理技术 65 因为 得输入扰动的协方差矩阵 智能信息处