卡尔曼滤波器介绍 --- 最容易理解

10 6 卡尔曼滤波器简介卡尔曼滤波器简介 本节讨论如何从带噪声的测量数据把有用信号提取出来的问题 通常 信号的频 谱处于有限的频率范围内 而噪声的频谱则散布在很广的频率范围内 如前所述 为 了消除噪声 可以把 FIR 滤波器或 IIR 滤波器设计成合适的频带滤波器 进行频域滤 波 但在许多应用场合 需要进行时域滤波 从带噪声的信号中提取有用信号 虽然 这样的过程其实也算是对信号的滤波 但所依据的理论 即针对随机信号的估计理论 是自成体系的 人们对随机信号干扰下的有用信号不能 确知 只能 估计 为了 估计 要事先确定某种准则以评定估计的好坏程度 最小均方误差是一种常用的 比较简单的经典准则 典型的线性估计器是离散时间维纳滤波器与卡尔曼滤波器 对于平稳时间序列的最小均方误差估计的第一个明确解是维纳在 1942 年 2 月首 先给出的 当时美国的一个战争研究团体发表了一个秘密文件 其中就包括维纳关于 滤波问题的研究工作 这项研究是用于防空火力控制系统的 维纳滤波器是基于最小 均方误差准则的估计器 为了寻求维纳滤波器的冲激响应 需要求解著名的维纳 霍 夫方程 这种滤波理论所追求的是使均方误差最小的系统最佳冲激响应的明确表达式明确表达式 这与卡尔曼滤波 Kalman filtering 是很不相同的 卡尔曼滤波所追求的则是使均方 误差最小的递推算法递推算法 在维纳进行滤波理论研究并导出维纳 霍夫方程的十年以前 在 1931 年 维纳和 霍夫在数学上就已经得到了这个方程的解 对于维纳 霍夫方程的研究 20 世纪五十年代涌现了大量文章 特别是将维纳滤 波推广到非平稳过程的文章甚多 但实用结果却很少 这时正处于卡尔曼滤波问世的 前夜 维纳滤波的困难问题 首先在上世纪五十年代中期确定卫星轨道的问题上遇到了 1958 年斯韦尔林 Swerling 首先提出了处理这个问题的递推算法 并且立刻被承认 和应用 1960 年卡尔曼进行了比斯韦尔林更有意义的工作 他严格地把状态变量的概 念引入到最小均方误差估计中来 建立了卡尔曼滤波理论 空间时代的到来推动了这 种滤波理论的发展 维纳滤波与卡尔曼滤波所研究的都是基于基于最小均方误差准则最小均方误差准则的估计问题的估计问题 维纳滤波理论的不足之处是明显的 在运用的过程中 它必须把用到的全部数据 存储起来 而且每一时刻都要通过对这些数据的运算才能得到所需要的各种量的估值 按照这种滤波方法设置的专用计算机的存储量与计算量必然很大 很难进行实时处理 虽经许多科技工作者的努力 在解决非平稳过程的滤波问题时 给出能用的方法为数 甚少 到五十年代中期 随着空间技术的发展 这种方法越来越不能满足实际应用的 需要 面临了新的挑战 尽管如此 维纳滤波理论在滤波理论中的开拓工作是不容置 疑的 维纳在方法论上的创见 仍然影响着后人 五十年代中期 空间技术飞速发展 要求对卫星轨道进行精确的测量 为此 人 们将滤波问题以微分方程表示 提出了一系列适应空间技术应用的精练算法 1960 年 和 1961 年 卡尔曼 R E Kalman 和布西 R S Bucy 提出了递推滤波算法 成功地将 状态变量法引入到滤波理论中来 用消息与干扰的状态空间模型代替了通常用来描述 它们的协方差函数 将状态空间描述与离散时间更新连系起来 适于计算机直接进行 运算 而不是去寻求滤波器冲激响应的明确公式 这种方法得出的是表征状态估计值 及其均方误差的微分方程 给出的是递推算法递推算法 这就是著名的卡尔曼理论 或称卡尔 曼 布西滤波 从维纳 霍夫方程来看 维纳滤波算法是十分低效的 这种算法要求设置大量存储 器保存过去的测量数据 新的一个数据来到后 要进行刷新刷新 重新计算自相关和互相 关序列 再者 求解这个方程需要耗费大量机时对高阶矩阵求逆高阶矩阵求逆 因此 维纳滤波算 法难以用于实时处理中 尤其是无法用于军事 航空航天等领域 在维纳滤波中 必须首先求解维纳 霍夫方程 得到有限长度的冲激响应序列 在 这个意义上 维纳滤波器属于维纳滤波器属于 FIR 滤波器的范畴滤波器的范畴 卡尔曼滤波不要求保存过去的测量数据 当新的数据到来时 根据新的数据和前卡尔曼滤波不要求保存过去的测量数据 当新的数据到来时 根据新的数据和前 一时刻的诸量的估值 借助于系统本身的状态转移方程 按照一套递推公式 即可算一时刻的诸量的估值 借助于系统本身的状态转移方程 按照一套递推公式 即可算 出新的诸量的估值 这一点说明卡尔曼滤波器属于出新的诸量的估值 这一点说明卡尔曼滤波器属于 IIR 滤波器范畴 这就是说 与维滤波器范畴 这就是说 与维 纳滤波器不同 卡尔曼滤波器能够利用先前的运算结果 再从当前数据提供的最新信纳滤波器不同 卡尔曼滤波器能够利用先前的运算结果 再从当前数据提供的最新信 息 即可得到当前的估值 卡尔曼递推算法大大减少了滤波装置的存储量和计算量 息 即可得到当前的估值 卡尔曼递推算法大大减少了滤波装置的存储量和计算量 并且突破了平稳随机过程的限制 使卡尔曼滤波器适用于对时变信号的实时处理 因并且突破了平稳随机过程的限制 使卡尔曼滤波器适用于对时变信号的实时处理 因 此 卡尔曼滤波器在应用上有更加广泛的可能性和更加美好的前景 此 卡尔曼滤波器在应用上有更加广泛的可能性和更加美好的前景 请读者回顾第 2 章的两个例子 即例 2 5 1 和例 2 6 5 这两个例子有助于理解 维纳滤波和卡尔曼滤波的主要区别 例 2 5 1 用 FIR 滤波器实现 5 点滤波 这有点像 维纳滤波 例 2 5 2 则用 IIR 滤波器实现同样的功能 而且滤波效果还稍好 在这个 例子中 利用前一个时刻的滤波输出 再添上当前的输入 新息 即可递归地求出 当前的滤波输出 这有点像卡尔曼滤波 为了导出维纳滤波器和卡尔曼滤波器 以下各小节先介绍两种估计器 非递归估 计器和递归估计器 然后依次介绍标量维纳滤波 向量维纳滤波 标量卡尔曼滤波和 向量卡尔曼滤波 重点是对标量卡尔曼滤波算法的推导 整个过程不涉及繁琐的数学 推导 10 7 非递归估计器非递归估计器 设表示慢变化 相对于叠加于其上的噪声 的时变信号 信号的测量值x 与成线性关系 并加上均值为零而方差为的白色噪声成分 这个噪 iyx 2 v iv 声成分源自测量或其他原因引起的随机误差 这样就有 ivxiy 10 7 1 这里把信号看作是均值为 方差为的随机变量 人们能够测到的信xiyE 2 v 号是 由于噪声的存在 人们只能够估计值 而永远无法直接测到有用信号 iyx 的平均值是一个最好的估值 如果用如下的阶 FIR 滤波x iy iyEx N 器对信号进行滤波 iy N n iyih N x 1 1 10 7 2A 式中 是个数据 则滤波值可以被作为的估计值 由 2 1 Nyyy N x x 于的均值为零 故随着平均次数的增大 中的随机噪声成分趋于零 若 ivN x 则1 2 1 Nhhh xivNx N iy N x N i N i 1 1 1 0 1 0 10 7 3 所以 FIR 滤波器可以作为一个估计器 estimator 随着平均次数的增大 估计N 值无限地接近真值 这样的估计称为无偏估计 unbiased estimation 估计器的均方误差为 2 1 0 1 0 1 xivx N Ep N i N i e Niv N E v N i 1 22 1 0 10 7 4 由此可见 经滤波后 估计量的方差是原信号方差除以 增大值可以提高估计NN 器质量 但值不能过大 因为估计器本质上是一个低通滤波器 值增大时 有NN 用信号的细节 高频分量 也将被滤除了 上述估计器只是简单地进行求和 平均运算 所依据的准则是使平均误差最小 而不是均方误差最小 10 3 节已导出基于 LME 准则的最佳系统方程 即 W H 方程 10 3 6 基于这个方程 曾导出 10 5 节的维纳滤波器 以这个滤波器作为估计器 就得到均方误差最小的非递归估计器 在目前的场合 输入信号已按式 10 7 1 定 义为 即变量代表输入信号 而则代表期望信号 所以 在对非 ivxiy yx 递归估计器进行分析前 应将式 10 3 6 修改为下式 1 0 110 N k xyyy N llrklrkh 10 7 5A 相应的矩阵方程为 1 1 0 1 1 0 0 2 1 2 0 1 1 1 0 Nr r r Nh h h rNrNr Nrrr Nrrr xy xy xy yyyyyy yyyyyy yyyyyy 10 7 6 这里 假定信号是平稳过程 故自相关矩阵是对称的 应该记住 被估计量是式 10 7 1A 中的 而估计量则由式 10 7 2 算出 x x 估计误差为 N i iyihxxxe 1 10 7 7 方差为 2 eEpe 10 7 8 在按 LME 准则导出 W H 方程时 曾令 0 2 1 jyiyihxE jh p N i e 10 7 9 Mj 2 1 由此得 NjjeyE 2 1 0 10 7 10 式中 是估计误差 在估计理论中 把称为正交原理 它xxe 0 jeyE 的意思是 误差与被测采样值的乘积在期望 即平均值 意义上等xxe jy 于零 根据这个式子 由 1 2 exEiyihxeEeEp N i e 得到对应于上述最优解的最小均方误差 因此有 1 2 N i e ixyEihxEp 或 N i xye irihxEp 1 2 10 7 11 其中 是互相关函数 irxy ixyEirxy 10 7 12A 这样 用方程组 10 7 5A 估计方程 10 7 2A 以及由式 10 7 11 给出的最小均方误差 就得到了估计问题的完整解 这三个式子的矩阵形式为 xyyy rhr 10 7 5B 式中 是自相关矩阵 和是列向量 yy r MM h xy r 1 M 方程 10 7 5B 的形式解为 xyyy rrh 1 10 7 13 估计方程可写为 yhx T 10 7 2B 这里和是列向量 是行向量 把式 10 7 13 代入式 10 7 2B 由 yy ry 1 M T h 于矩阵是对称的 故的估计为 yy rx 10 7 14 yrrx 1 yy T xy 用同样的方法可以得到最小均方误差为 10 7 15 e p 2 xE xy 1 yy T xy rrr 上面分析了基于维纳滤波器的标量估计器 应该指出 在推导中还没有使用关 系式 10 7 1 即 因此所得结论更具有一般性 这个结论说明 kvxky 如果数据采样值以某种形式包含了随机未知变量 即信号 2 11 Miy x 那末 用维纳滤波器可在采样值上实现最佳线性滤波 给出的估计 x 现在假设 就这种特殊情况对非递归估计器进行分析 这里 kvxky 是加在信号上的白色噪声 假设其均值为零 噪声样点之间不相关 即方差 kvx 为 0 2 kj kj kvjvE v 10 7 16 噪声与信号之间也不相关 故 此外 为了简化讨论 设 0 jxvE0 xE 因此有 22 x xE 现在我们观察式 10 7 5A 的自相关函数和互相关函数 一般来说 自相关函 数为 jvxivxEjyiyEjiryy 22 ji vx 10 7 17 其中 是克罗内克 Kronecker 符号 其定义为 ji 1 0 ji ji ji 对于平稳随机过程 由于自相关函数只取决于位移绝对值 即 ijrjir xyxy 故自相关函数变为单变量函数 krxy 另一方面 互相关函数为 22 xxy xEjxyEjr 10 7 18 将式 10 7 17 和式 10 7 18 代入式 10 7 5A 并假定信号是平稳随机过程 得 22222 22222 22222 2 1 2 1 2 1 xvxxx xxvxx xxxvx Nhhh Nhhh Nhhh 或 2 1 22 2 1 22 2 1 22 2 1 x N i xv x N i xv x N i xv ihNh ihh ihh 10 7 19 将方程组两边分别相加 得 2 1 22 x N i xv NihN 于是有 22 2 1 vx x N i N N ih 把这个结果代入式 10 7 19 的各式中得 22 2 2 1 vx x N N Nhhh 10 7 20A 设 则 22 xv N Nhhh 1 2 1 10 7 20B 把这个结果代入式 10 7 2A 可得 N i iy N x 1 1 10 7 21 利用式 10 7 11 和式 10 7 20 就能得到对应的最小均方误差 即 N p v e 2 10 7 22 10 8 递归估计器递归估计器 上节论述的非递归估计器和维纳滤波器一样 是很难实现的 主要原因是 对于时变系统 冲激响应与冲激出现时间有关 每计算一个估值 都必须 重新计算冲激响应 为此 不得不耗费大量机时求自相关矩阵之逆 重新解 W H 方程 需要用大量存储器存放自相关矩阵和互相关向量 对于时变系统 这些统 计量还需要不断刷新 必须事先确定处理过程的数据采样值的数目 M 若由于某种原因改变了 例如需要得到更多的数据 那末全部计算都M 必须重新进行 再者 非递归估计属于批处理 batch processing 方式 其特点是为了处理好一 个数据 需要输入当前时 刻的数据和此前若干个数据此前若干个数据 显然 这是低效的 情况变化时 需要停下来重新计算 还有一个问题 即 系统如何获知情况已发生变化 与此相反 可以设想 改用递归估计会更为合理 递归估计属于序贯处理 sequential processing 方式 其特点是根据当前时刻的输入数据 结合已求出的前前 一个时刻的估计值一个时刻的估计值 即可算出当前的估计值 这种算法的优点是高效 节省存储器 但这还不是事情的全部 如果递归估计器配上反馈环节 对误差进行自我修正 就构 成能付诸实用的卡尔曼滤波器 本节的目的是根据对最优非递归估计器的分析结果 导出等效的最优递归估计器 下面仍然按照以前的做法 根据给出的逐次采样值来作出线 kvxky 性估计 即 k i iyihkx 1 并使均方误差尽可能地小 上一节已得到这个问题的非递归解 现 2 xxEpe 在把它从新列出 即对于个采样值有 k k i iyihkxx 1 10 8 1 式中 其对应的均方误差为 k ih 1 k kxxEkpp v e 2 2 10 8 2 式中 注意 符号表示参数的第次估计 也就是在一批个采 2 2 x v kxxkk 样值处理后的估计 式 10 8 2 中的也作同样的解释 kp 下面进行推导 从下标为的量求下标为的量 以便实现递归计算 k1 k 对于个采样值 它的估计和对应的均方误差是1 k 1 1 1 k i iyihkx 10 8 3 式中 1 1 k ih 1 1 2 k kp v 10 8 4 因为表示所使用的采样值的数目 它是一个变化的量 因此用符号来表示k kih 更为确切 根据式 10 8 1 和 10 8 2 得 2 v kpkih 同样从式 10 8 3 和 10 8 4 得 2 1 1 v kpkih 上两式之比为 11 1 1 1 1 kk k kih kih kp kp 于是得到 2 1 1 1 v kp kp kp 10 8 5 这是一个差分方程 可以对均方误差进行递归计算 当给定后 就能得到 kp 然后又能得到 如此等等 1 kp 2 kp 对于这种特殊情况 根据式 10 8 3 有 kvxky 1 1 1 1 1 1 1 ky k iy k kx k i 运用式 10 8 1 有 1 1 1 1 1 ky k kx k k kx 再利用式 10 8 5 的关系可得 1 1 1 1 2 ky kp kx kp kp kx v 10 8 6 这是一个递归估计式 它和式 10 8 5 一起组成了递归算法 图 10 8 1 示出递归计算过程 整个过程不断地生成的最佳线性均方x 估计 同时给出对应的均方误差 注意 当非常大时 1 kpk 趋于零 kp 递归过程开始时 要赋初值 我们必须按非递归算法 依据单 个观测值算出第一个估计值 1 x 例例 10 8 1 设 给定 试推导的递归估计序列 2 2 v x 解 解 先通过式 10 8 1 给出的非递归估计求得的 x 第一个估计值 它对于第一个数据采样值 即 给出第一个估 1 y 计值为 根据式 10 8 2 就能得到对应的均方误差为 图图 1 3 1 1 yx 10 8 1 递归计算过程递归计算过程 2 3 1 1 v p 现在就来进行递归估计 由式 10 8 5 得 4 3 1 1 3 1 1 1 1 2 2 2 2 v v v p p p 然后由式 10 8 6 得到 2 4 1 1 4 3 2 yxx 由于 因此上式也可以写成 1 3 1 1 yx 2 4 1 1 4 1 2 yyx 它与非递归处理一致 同理 可以得到以及第三个数据采样后的估计值 2 5 1 3 v p 3 3 2 2 3 3 2 y p x p p x v 即 3 5 1 2 5 4 3 yxx 如此等等 记 1 1 kp kp ka 2 1 1 v kp kb 10 8 7 于是 式 10 8 6 变为 1 1 1 1 kykbkxkakx 10 8 8 根据式 10 8 7 和 10 8 5 易证参数和有如下关系 ab 1 1 1 kbka 10 8 9 因此式 10 8 8 也可改写为 1 1 1 kxkykbkxkx 10 8 10 式 10 8 10 有重要意义 它表明这个系统是一个反馈系统 具有自我修正功 能 式中右边第二项代表修正项 是一个时变修正因子 1 kb 是新的数据测量值与上次估计值之差 新的一个估计值是 1 kxky 1 kx 上次求出的估计值与修正项之和 式 10 8 8 和 10 8 10 的实现框图如图 10 8 2 和图 10 8 3 所示 图图 10 8 2 由非递归估计器导出的由非递归估计器导出的 图图 10 8 3 图图 10 8 2 的另一种形的另一种形 式式 递归估计器递归估计器 10 9 最优递归估计器最优递归估计器 标量卡尔曼滤波器标量卡尔曼滤波器 10 9 1 系统模型系统模型 1960 年 卡尔曼首先证明后来被称为卡尔曼滤波器的四个基本方程 本节将针对 标量信号导出这些递归方程 卡尔曼滤波器要求被处理的信号是一个广义马尔科夫过程广义马尔科夫过程 这种信号的特点是 从当前状态可以唯一地确定下一个时刻的状态 即状态可以通过转移矩阵转移到下一状态可以通过转移矩阵转移到下一 个状态个状态 一般的信号都能满足这个要求 为分析方便 先假设随机信号是一个 kx 由零均值白噪声序列激发一个递归系统而产生的一阶自回归过程 图 10 9 1 示 kw 出系统模型 左边是信号模型 右边是测量模型 其中 是测量过程引入的系 kv 统噪声 假定是均值为零的白噪声 在测量模型中 将输入信号乘以因子 kvc 这样能使所得的结果推广到向量信号的场合 图图 10 9 1 系统模型系统模型 由图 10 9 1 知信号关于时间的变化满足下列动态方程 1 1 kwkxkx 10 9 1 设初始采样值 即时 又设随机驱动定0 0 x0 k0 kx0 kw 义为 0 kwE jk jk jwkwE w 0 2 10 9 2 式 10 9 1 定义的自回归过程有以下统计特性 kx 0 kxE 2 2 22 1 0 w xx pkxE 10 9 3 0 x j x pjpjkxkxE 10 9 4 式 10 9 3 的证明如下 相隔单位时间的的两个采样值的自相关函数为j kx jkxkxEjpx 对于 有0 j 22 1 1 0 kwkxEkxEpx 1 1 222 kwEkxE 1 1 2 kwkxE 根据式 10 9 2 可推知上式右边第三项为零 故 0 0 0 2 wxx ppp 于是 引入 就能得到式 10 9 3 在滤波器处于稳 2 0 xx p 2 0 ww p 态并已运行很长时间的情况下 这个结果是正确的 因此输出特性是平稳的 即 1 22 kxEkxE 式 10 9 4 的证明如下 对于 有1 j 1 1 kwkxkxEkxkxEpx 0 2 x pkwkxEkxE 对于 有2 j 1 1 2 2 kwkxkxEkxkxEpx 1 1 kwkxEkxkxE 0 1 2 xx pp 用同样的方法可以得到取其它值时的结果 取负值时 这个结论也成立 于是 jj 式 10 9 4 得证 从以上证明可以看出 对所有的 都不为零 而且随的增加而递j jpx j 减 当为负值时 的值在正 负之间摆动变化 jpx 在处理过程中 参数作为一个反馈量 起时间常数的作用 值增大时 信 号的自相关性增大 kx 现在 再看图 10 9 1 右边的线性观察模型 它可以被描述为 kvkcxky 10 9 5 式中包含时变信号以及表示观察参数 或测量参数 的因子 以后将会看到 kxc 因子在把标量结果变换到向量信号的场合时非常有用 式中和前面规定的一c kv 样 是一个加在信号上的独立的白噪声 它的均值为零 方差为 2 v 10 9 2 算法推导算法推导 由图 10 8 2 得递归估计器的形式为 1 kykbkxkakx 10 9 6 式中 右边第一项表示加权的先行估计 第二项则是加权的现行数据采样值 注意 上一节中的是根据成批数据中的个数据而作出的参数的估计 而此处的 kxkx 则是根据先行估计和时刻唯一的一个数据而对作出的 kx 1 kxk ky kx 估计 下面用式 10 9 6 来求最佳估计 最佳 的意义仍然是使均方误差最小 此时 我们有和两个参数 它们通过使均方误差最小来求得 为此 令 ka kb 2 keEkp 10 9 7 最小 其中为估计误差 kxkxke 把表达式 10 9 6 代入式 10 9 7 得 kx 2 1 kxkykbkxkaEkp 10 9 8 分别对和求导 得 ka kb 0 1 1 2 kxkxkykbkxkaE ka kp 10 9 9 0 1 2 kykxkykbkxkaE kb kp 10 9 10 或写为另一种形式 0 1 kxkeE 10 9 11 0 kykeE 10 9 12 根据式 10 9 11 可得和的关系为 ka kb 解说 10 9 1 1 kcbka 10 9 13 把这个关系式代入式 10 9 6 得 1 1 kxckykbkxkx 10 9 14 第一项表示没有其它任何附加信息加入时的最佳估计 因此 它是 1 kx kx 根据先行的观察值所作的预测 第二项是修正项 其中 是根据前一个 1 kxc 时刻的估计值求出的对当前输入数据的估计值 是当前的实测数据 1 kx ky 二者之差称为余差 余差乘以增益因子构成一个修正项 它与第一项之和 1 kx 就是当前时刻的估计值 图 10 9 2 综合了以上分析结果 这里 可以证明增k kx 益因子为 解说 10 9 2 1 1 22222 22 kpcc kpc kb wv w 10 9 15 均方误差为 1 2 kb c kp v 10 9 16 可见和有直接的联系 注意 当和时 从式 kp kb1 c 0 2 w 10 9 15 和式 10 9 16 就可以得到式 10 8 5 的结果 图图 10 9 2 最优递归估计器 滤波器 最优递归估计器 滤波器 如上所述 系统开始工作时需要用第一个观察值来产生非递归估计 见例 1 x 10 8 1 也可以用一个估计值代替第一个观察值来作出估计 为此 求下式 0 x 2 0 0 xkxEp 10 9 17 取最小值时的值 于是有 0 x 0 0 2 0 0 xkxE x p 10 9 18 即 0 kxEx 由此可见 的最佳估计就是的平均值 0 x kx 例例 10 9 1 利用上面的结果 并设 就能按如下步骤启动系统 0 kxE 因为 所以用第一个数据采样值就可以从式 10 9 14 中得到 0 0 x 1 y 这里 为了求得 可用如下正交关系 1 1 1 ybx 1 b 0 1 1 1 yxxE 式中 其中取 以代入 并取平均 1 1 1 vxy 1 c 1 1 1 ybx 即得 22 2 1 vx x b 再设 从式 于是 把 22 wv 2 1 2 22 2 vx 67 0 3 2 1 b 此值代入式 10 9 16 得 这样就得到了用式 10 9 15 计算 2 3 2 1 v p 所需的全部数据 于是 根据上述求得的数值可得 2 b 57 0 7 4 1 1 2 222 22 p p b wv w 再根据式 10 9 16 求得均方误差为 把代入式 10 9 15 7 4 2 2 v p 2 p 又可得 于是 如此等等 随着的增562 016 9 3 b 2 562 0 3 v p k 加 逐渐达到稳态值 稳态值可以通过把式 kppkpkp 1 p 10 9 16 代入式 10 9 15 求得 即 023 422 vv pp 解上述方程得 可见上面计算得到的与极限值 即稳态值 2 56 0 v p 3 p 非常接近 2 0 56 v 式 10 9 14 到式 10 9 16 构成了一个完整的算法 为了使这些结果能推广 到向量信号情况 最好把这些式子从新排列成如下形式 递归滤波估计器递归滤波估计器 1 1 kxckykbkxkx 10 9 19 滤波器增益滤波器增益 12 1 2 1 v kpckcpkb 10 9 20 其中 22 1 1 w kpkp 10 9 21 均方误差均方误差 11 kpkcbkpkp 10 9 22 估计方程 10 9 19 与原先的方程 10 9 14 完全一样 而式 10 9 15 和式 10 9 16 这两个式子在引进新的量以后可以用式 10 9 20 到 10 9 22 三 1 kp 个式子来表示 对于由式 10 9 1 给出的信号模型 即 1 1 kwkxkx 以及由式 10 9 5 给出的观察模型 即 kvkcxky 上述一组方程构成了标量 一维 卡尔曼滤波器 这种类型的递推估计大约在 1960 年开始发展起来 在这方面 R E 卡尔曼的贡献最为突出 10 10 向量卡尔曼滤波器向量卡尔曼滤波器 10 10 1 信号向量和数据向量信号向量和数据向量 上节讨论了标量卡尔曼滤波器 在这种最简单的卡尔曼滤波器中 不断地被估 计的信号只有一个 而在这个过程中用以进行校正的测量数据也只有一个 但在实际 应用中 往往需要对几个信号同时进行估计 为此 需要实测的数据也不止一个 例 如 在雷达对飞行物进行测距时 可以用标量卡尔曼滤波器对距离进行估计 每次估 计时 只用一个实测数据对距离的估计值进行修正 显然 这种简单的算法不可能得 到很高的精度 为了提高估计精度 较好的做法是对飞行物的距离和速度同时进行估 计 而用一个实测数据进行修正 在这种情况下 涉及的信号有两个 即距离与速度 本节先介绍信号向量和数据向量的概念 然后 下一节将把先前对标量卡尔曼的分析 推广到向量卡尔曼滤波器 例例 10 10 1 假设要对个独立的信号同时进行估计 即同时预测 个信qq 号在时刻的采样值记作 又假定每个信号是由它们本身k 21 kxkxkx q 的一阶自回归过程产生的 例如 第 个信号是由第 个白色的 均值为零且独i kxii 立于其它所有信号的噪声所激发 即 kwi q ikwkxakx iiii 21 1 1 10 10 1 我们把这些信号和相应的驱动源分别用向量表示为 kx 2 1 kx kx kx q kw 2 1 kw kw kw q 10 10 2 于是 可将标量方程 10 9 1 改写为如下的一阶向量方程 1 1 kkkwAxx 10 10 3 式中 和是 列向量 是一个 矩 kx 1 kx 1 kw1 qAqq 阵 在本例中是一个对角阵 即 q 2 1 0 0 0 0 0 a a a A 10 10 4 例例 10 10 2 考察雷达跟踪问题 这对于理解卡尔曼滤波器的工作很有帮助 设时刻时k 雷达与被跟踪飞行器的距离为 秒后 也就是在时刻时距离为 kR T1 k 表示在一个扫描间隔产生的样点之间的时间 为平均距离 1 kR TR 等表示与平均值的偏差 人们感兴趣的是估计这些偏差 它们可 k 1 k 以看成是统计意义上平均值为零的随机量 若飞行器的径向运动速度为 而 k 也不是太大 那末 用一阶近似可得距离方程为T 1 kTkk 10 10 5 同理 对于加速度 也有如下加速度方程 ku 1 kkkTu 10 10 6 若设是一个均值为零的 平稳的白噪声过程 那末加速度的平均值为零而且互 ku 不相关 即 但它却具有某个已知的方差 这种加速0 1 kukuE 22 u kuE 度可以是由于突然产生的阵风及发动机驱动力短时间的不规则变化而引起的 量 也是一个白噪声过程 把它代入式 10 10 6 得 1 kTuku 1 1 kukk 10 10 7 现在 定义一个由两个分量组成的向量信号 它的一个分量是距离 kx 1 kx 有 另一个分量是径向速度 有 根据这个定 1 kkx 2 kx 2 kkx 义 式 10 10 5 和 10 10 7 可以写为 1 211 kTxkxkx 1 122 kukxkx 或把它们归入一个向量方程 得 0 1 0 1 1 1 1 2 1 2 1 kukx kxT kx kx 10 10 8 1 kxA kx kw 对于时变系数的方程 可以通过定义时变矩阵时变矩阵来处理 这种时变矩 1 kkA 阵也称作系统转移矩阵 系统转移矩阵 下面来讨论数据向量 假设在时刻对信号向量的估计过程中 同时产生k x k 个含有噪声的测量值 如果把这些测量采样值记为 就r 21 kykyky r 得到如下一组数据 2222 1111 kvkxcky kvkxcky kvkxcky rrrr 10 10 9 其中 项表示加在信号上的噪声 表示若干个参数 它和式 kvj r ccc 21 10 9 5 引进的类似 通过定义有个分量的向量和 这个方程组可cr ky kv 以写成向量形式 按照前面具有个分量的向量的定义 我们有数据向量q kx kkkvCxy 10 10 10 式中 和是 列向量 是 列向量 是一个 ky kv1 r kx1 qC 的观察矩阵 本例中 若设 观测量的数目往往少于被估计量的数qr qr r 目 则q 10 10 2 向量问题的表示向量问题的表示 第 10 9 节讨论了标量卡尔曼滤波器的算法 对于向量信号 基本原理是相同的 只需将标量信号改成向量 其它参数改为矩阵或向量即可 回到基本问题 一个信号向量满足下列一阶向量动态方程 x k 1 kkkwAxx 10 10 12 它可以从含有噪声的测量值向量中检出 ky kkkvCxy 10 10 13 这两个方程都可以按上节讨论的方法得到 现在的问题是如何产生的 最佳 线性估计 滤波值 以及如何求 kx kx 出 最佳 预测值 这里 代表由时刻的条件求时 1 kkx 1 kkx1 k 刻的的估计值 而所谓 最佳 的意思是估计器同时使每个信号分量的均方k kx 误差都最小 例如在滤波运算中使每个均方误差 2 1 2 qikxkxE ii 10 10 14 都达到最小 这个问题在形式上和以前处理满足一阶动态方程的单一时变信号的情况一致 处理过程与单一信号的情况非常相似 只不过现在用向量形式来表示 由于已经有了 一维 标量 情况的结果 所以可以采用标量运算与向量运算的等价关系把它推广到 多维 向量 系统 表 10 10 1 列出这种等价关系 表中 上标表示矩阵的转置运T 算 表示矩阵的逆运算 1 表表 10 10 1 标量到矩阵的变换标量到矩阵的变换 标量 矩阵 ba BA abAB ba2 T ABA ba 1 1 BA 在上一节中 我们已经知道从单一信号到向量信号的变换时 系数变成系统a 矩阵 而数据系数则变成观察矩阵 下面讨论其它有关量的变换 AcC 从观察噪声方差到观察噪声协方差矩阵 即众多信号的公共方差 的变换关系 为 T2 1 2 1 1 2 kkEkkvE vv vvR 10 10 15 式中 用了表 10 10 1 中的第三个关系 其中令 例如 对于两个信号的情况 1 b 有 v v v v 2 2 12 21 2 1 kvEkkE kkEkvE kR 2 v2 2 2 v2 1 2 v1 2 2 v1 1 10 10 16 同理 对于系统噪声方程有如下变换关系 T2 1 2 1 1 2 kkEkkwE ww wwQ 10 10 17 这里表示系统噪声协方差矩阵 噪声过程互不相关时 它的非对角线元素的值 kQ 全为零 单一信号均方误差变成误差协方差矩阵的关系为 T2 11 1 kkEkkeEkPkPeeP 10 10 18 对于两个信号的例子 有 2 212 21 2 1 keEkekeE kekeEkeE kP 2 22 1 1 21 1 kpkp kpkp 10 10 19 注意 式中对角线上的各项是按式 10 10 14 得到的各自的均方误差 协方差矩阵 的元素代表第个信号的误差对第 个信号的影响 kP ji P ji 10 10 3 向量卡尔曼滤波器向量卡尔曼滤波器 现在 可以把式 10 9 19 到 10 9 22 的标量卡尔曼滤波器算法变换为相应的 向量卡尔曼滤波器算法 参考这些式子和以上的讨论 可以直接写出这个算法的向量 和矩阵方程如下 估计器估计器 1 1 kkkkkxCAyKxAx 10 10 20 滤波器增益滤波器增益 1T 1 T 1 kkkkRCCPCP 10 10 21 式中 1 1 T 1 kkkQAAPP 10 10 22 误差协方差矩阵误差协方差矩阵 11 kkkkkPCKPP 10 10 23 以上各式子构成了向量卡尔曼滤波器 它的模型可由下列两个方程来描述 状态方程 状态方程 1 1 kkkwAxx 10 10 24 观测方程观测方程 kkkvCxy 10 10 25 第 10 9 1 节已对标量信号讨论了上述两个方程 在式 10 9 10 中 代表标量卡尔曼滤波器增益 而在式 10 10 20 中 kb 改用来代表向量卡尔曼滤波器增益 这个符号是卡尔曼滤波器增益矩阵通常采 kK 用的符号 式 10 10 20 10 10 23 是卡尔曼滤波器的四个基本方程四个基本方程 请读者参看第 10 10 1 关于信号向量和数据向量的定义 从矩阵和向量的维数验证这四个向量方程写 法的正确性 上述四个方程的递归性递归性是卡尔曼滤波器最有意义的特点之一 例如 将式 10 10 21 和式 10 10 22 代入式 10 10 23 分别置换式中的和 可 kK 1 kP 以看出该式是一个非线性差分方程 因此 用封闭形式表示这个被称为黎卡堤 Ricatti 方程的解 以得到误差矩阵 是非常困难的 但是 通过对以上四 kP 个方程的递归运算 误差矩阵的求解是非常自然和容易的 卡尔曼滤波器的递归运算过程大致上如图 10 10 1 所示 10 10 1 卡尔曼滤波器的递归运算示意图卡尔曼滤波器的递归运算示意图 图 10 10 1 的意思是 在时刻 算出经过修正的估计向量 1 k 1 1 kkx 然后用这个估计向量去预测时刻的估计向量 时刻的信号观测向量k 1 kkxk 是 用对进行修正 得到时刻的估计向量 根 ky ky 1 kkxk kkx 据求时刻的估计向量 再用该时刻的观测向量 kkx1 k 1 kk x 进行修正 得 等等 整个过程可以描写为 1 ky 1 1 kkx 估估估估估估估估 具体地说 整个递归运算包括两个互相联系的过程 第一个递归过程计算信号估 计值 其中所用的协方差矩阵由第二个递归过程提供 基于式 kx kP 10 10 20 10 10 23 我们可以用图 10 10 2 和 10 10 3 分别概括这两个过程 其中 理解第一个递归过程尤为重要 下面对此加以说明 假定已经知道时刻的估计值以及协方差矩阵 先1 k 1 x k 1 kP 估计时刻的协方差矩k 阵 1 P 乘以系统转移矩阵 得到 这是时刻的估 1 x kA 1 kxA1 k 计值对时刻估计值的贡献 此即时刻对时刻的估计值的预k kx1 kk 测 中还有一个修正量 为了计算该修正量 将预测值乘以观察矩阵 kxC 得 此即时刻对时刻输入值的估计 1 kxCA1 kk 时刻的实际输入量是 而不是 二者之差称为余差k ky 1 kxCA residue 余差等于 1 kkkxCAye 将观测误差乘以卡尔曼增益 得 这是一个修正量 将 keK keK 估计值加上修正值 即得时刻的信号估计值 1 kxA keKk kx 通过卡尔曼增益对所估计的协方差矩阵进行修正 得到当前的协方 1 PP 差矩阵 P 重复以上递归过程 就得到信号估计值 滤波值 2 1 kkxx 图图 10 10 2 卡尔曼滤波器的递归运算过程卡尔曼滤波器的递归运算过程 1 求信号估计值求信号估计值 图图 10 10 3 卡尔曼滤波器的递归运算过程卡尔曼滤波器的递归运算过程 2 计算协方差矩阵计算协方差矩阵 卡尔曼滤波器采用递归算法 在进入递归循环之前 需要进行初始化 然后进 入上述四个基本向量方程的递归运算 在递归循环中 可以先计算某个方程 这种选 择并不重要 当点数超过某值 例如 滤波结果与上述选择无关 当k100 k 然 点数小时 滤波过程会受到影响 例如 初始的卡尔曼增益可能变得很大 根据 作者的实践 上述步骤比较合适 作为本章的总结 我们举出一个雷达测距例子 说明卡尔曼滤波器的工作过程 请读者结合可视化程序B0901领会卡尔曼滤波器的原理 程序的核心部分摘录如下