|
楼主 |
发表于 2023-6-5 14:19:15
|
显示全部楼层
附录-Kalman滤波算法推导
Kalman滤波算法相对于线性最小方差估计,拥有如下特点:
1. Kalman滤波有一系列必要条件: 状态模型和量测模型均为线性;状态噪声和量测噪声均为零期望白噪声;状态噪声、量测噪声和状态三者之间不相关;初值或者统计特性(期望和协方差矩阵)已知。当满足这些条件时,滤波结果在无偏最小方差意义上是最优的;反之如果不满足这些条件,则滤波结果不是最优的。
2.迭代算法。虽然其本质上也利用了从初始时刻到当前时刻的所有测量信息,但采用的是迭代方法,并不需要保存之前的测量量,适合于计算机处理,而且便于进行多维处理,及多输入,多输出系统的状态估计。
3.同时利用了状态和量测方程。在利用量测方程的基础上,进-步将状态方程也纳入滤波算法设计,从而同时利用了状态自身的变化规律和观测量,使得最大程度地提高估计精度。
4.适用于非平稳过程。由于有状态方程,状态统计特性的变化由状态方程和状态噪音实时确定,而状态噪音一般建模为平稳白噪音,因此,Kalman滤波 算法不仅适用于平稳过程,也适用于非平稳过程。
5.增益矩阵是调节加权权重的参数。当量测噪声方差矩阵增大时,增益矩阵减小,即分配给当前测量值的权重减小。
一、系统建模
建立状态方程与量测方程:
k时刻的状态值为Xk ; Ak为状态转移矩阵; Γk 为状态噪音系数矩阵; Wk 为状态噪音。
k时刻的测量值为Yk ; Ck 为量测矩阵; Vk 为量测噪声。
二、算法推导
仅基于状态方程进行预测可能导致误差发散,仅基于量测方程进行滤波无法适用于非稳态过程。因此,将两者结合起来就可以实现优势互补。
设定k时刻状态的预测估计结果为
k/k-1,量测滤波修正结果为 k/k,相应的估计偏差定义为:
基于状态方程进行预测:
估计偏差为:
设
,即上一时刻量测修正是无偏的,代入式(12)有
预测的估计偏差协方差矩阵为:
其中,设状态与状态噪声不相关,即
下面,再按照递推滤波器的架构,构建量测修正估计:
其中, Kk′ 和 Kk 为待定的加权系数矩阵,显然,这里采用的是线性估计。在式(g)中之所以用 X^k/k−1 和 Zk 进行加权,而不是用 X^k−1/k−1 ,是因为通过预测, X^k/k−1 包含了状态方程的信息,因而, X^k/k−1 的精度可能会比 X^k−1/k−1 更高。剩下的问题是如何确定两个加权系数矩阵,这里采用无偏和小方差和最小方差准则,推导过程如下:
首先考虑无偏估计,有
考虑到 Xk 为任意随机向量,所以 E[Xk] 不可能恒为0,那么只能是其系数矩阵为0,即
再利用最小方差估计准则,有
式(g)中 Kk 为待定增益矩阵,考虑到式(g)为二次型(二次型是n个变量上的二次齐次多项式)的形式,肯定有极小值,因此,求 J 关于 Kk 的梯度,令其为0,即为极小值点。
其中用到了如下公式:
由式(k)可得:
将式(i)和(m)代入式(g)中得到:
式(h)保证了量测修正估计也是无偏的,即具有无偏保持性。下面确定其估计偏差协方差矩阵。
将式(m)代入(o)可得到:
三、算法总结
将上述推导结果进行归纳总结:
1.系统建模
2.初始条件
3.滤波算法
状态预测:
量测更新:
|
|