的卡尔曼滤波算法

自适应卡尔曼滤波

卡尔曼滤波发散的原因

如果卡尔曼滤波是稳定的，随着滤波的推进，卡尔曼滤波估计的精度应该越来越高，滤波误差方差阵也应趋于稳定值或有界值。但在实际应用中，随着量测值数目的增加，由于估计误差的均值和估计误差协方差可能越来越大，使滤波逐渐失去准确估计的作用，这种现象称为卡尔曼滤波发散。

引起滤波器发散的主要原因有两点：

（1）描述系统动力学特性的数学模型和噪声估计模型不准确，不能直接真实地反映物理过程，使得模型与获得的量测值不匹配而导致滤波发散。这种由于模型建立过于粗糙或失真所引起的发散称为滤波发散。

（2）由于卡尔曼滤波是递推过程，随着滤波步数的增加，舍入误差将逐渐积累。如果计算机字长不够长，这种积累误差很有可能使估计误差方差阵失去非负定性甚至失去对称性，使滤波增益矩阵逐渐失去合适的加权作用而导致发散。这种由于计算舍入误差所引起的发散称为计算发散。

针对上述卡尔曼滤波发散的原因，目前已经出现了几种有效抑制滤波发散的方法，常用的有衰减记忆滤波、限定记忆滤波、扩充状态滤波、有限下界滤波、平方根滤波、和自适应滤波等。这些方法本质上都是以牺牲滤波器的最优性为代价来抑制滤波发散，也就是说，多数都是次优滤波方法。

自适应滤波

在很多实际系统中，系统过程噪声方差矩阵Q和量测误差方差阵R事先是不知道的，有时甚至连状态转移矩阵 或量测矩阵H也不能确切建立。如果所建立的模型与实际模型不符可能回引起滤波发散。自适应滤波就是这样一种具有抑制滤波发散作用的滤波方法。在滤波过程中，自适应滤波一方面利用量测值修正预测值，同时也对未知的或不确切的系统模型参数和噪声统计参数进行估计修正。自适应滤波的方法很多，包括贝叶斯法、极大似然法、相关法与协方差匹配法，其中最基本也是最重要的是相关法，而相关法可分为输出相关法和新息相关法。

在这里只讨论系统模型参数已知，而噪声统计参数Q和R未知情况下的自适应滤波。由于Q和R等参数最终是通过增益矩阵K影响滤波值的，因此进行自适应滤波时，也可以不去估计Q和R等参数而直接根据量测数据调整K就可以了。

输出相关法自适应滤波的基本途径就是根据量测数据估计出输出函数序列

}{C k ，再由}{C k 推算出最佳增益矩阵K ，使得增益矩阵K 不断地与实际量测数

据}{C k 相适应。

.

Sage-Husa 自适应卡尔曼滤波是在利用量测数据进行递推滤波时，通过时变噪声估计估值器，实时估计和修正系统噪声和量测噪声的统计特性，从而达到降低系统模型误差、抑制滤波发散提高哦滤波精度的目的。

k k k k k w x x +Φ=--11,

k k k k v x H z +=

⎪⎪⎩⎪⎪⎨⎧=====0

)()(,)()(,)(T

j k kj

k T

k k k k kj k T

k k k k v w E R v v E r v E Q w w E q w E δδ Sage-Husa 自适应卡尔曼滤波算法可描述为

k k k k k z K x x ~ˆˆ1,+=- 111,1,ˆˆˆ----+Φ=k k k k k k q x x

k k k k k k r x

H z z ˆˆ~1,--=- 11,1,]ˆ[---+=k

T k k k k T k k k k R H P H H P K 11,11,1,ˆ-----+ΦΦ=k T k k k k k k k Q P P

1,)(--=k k k k k P H K I P

.其中，k r ˆ、K

R ˆ、k q ˆ和k Q ˆ由以下时变噪声统计估值器获得： )ˆ(ˆ)1(ˆ,1111k k k k k k k k x H z d r d r

++++-+-= )~~(ˆ)1(ˆ1,11111T k k k k T k k k k k k H P H z z d R d R ++++++-+-=

)ˆ(ˆ)1(ˆ,111k k k k k k k k x x d q d q

+++Φ-+-=

.)~~(ˆ)1(ˆ,1,1111111T k k k k k k T k T k k k k k k k P P K z z K d Q d Q ++++++++ΦΦ-++-=

式中：1

11+--=

k k b

b

d ，10<

基于极大似然准则的自适应卡尔曼滤波，通过系统状态方差阵和量测噪声方差阵实时估计系统噪声统计特性的变化，以保证滤波器更好地适应这种变化。极大似然估计从系统量测量出现概率最大的角度估计，其特点是不仅考虑新息的变

化，而且考虑新息协方差矩阵vk C 的变化。它的量测噪声协方差矩阵R

ˆ和系统噪声协方差矩阵Q

ˆ为： T k

k k k vk k H P H C R 1,ˆˆ--= 1,11,1

1ˆ---+-=ΦΦ-+∆∆=∑k k k T k k k T i k

N k i i

k

P P x

x N

Q

k k k k k k v K x x

x =-=∆-1,ˆˆ ∑+-==

k

N k i T i

i vk v

v N

C 1

1

式中：1,ˆ--=k k k k z

z v ，N 为平滑窗口的宽度。

扩展卡尔曼滤波

最初提出的卡尔曼滤波基本理论只适用于状态方程和量测方程均为线性的随机线性高斯系统。但是大部分系统是非线性的，其中还有许多事强非线性的。非线性估计的核心就在于近似,给出非线性估计方法的不同就在于其近似处理的思想和实现手段不同。近似的本质就是对难以计算的非线性模型施加某种数学变换,变换成线性模型,然后用Bayes 估计原理进行估计。进一步说,非线性变换到