本文在参考文献的基础上添加了自己的理解,若有不当之处,敬请指正
卡尔曼滤波以前接触过,但是没有仔细推导,这次参考文献仔细推导实现,也是第一次完全的通过vscode+markdown来完成写作。
卡尔曼滤波,也被称为线性二次估计(Liner Quadratic Estimation, LQE),可以作为平滑数据、预测数据、滤波器;本人理解:是一个观察→预测的过程,我接触到的卡尔曼滤波主要有KF(卡尔曼滤波,线性),EKF(扩展卡尔曼滤波,非线性),UKF(无迹卡尔曼滤波,强非线性)。此处,只研究基本的KF。
图1 来自老爷子的亲切凝视
- 参考:
- . python源码参考
- . kalman滤波器介绍和实例参考
- . kalman方法推导1
- . 极大似然估计
- . 矩阵迹求导
- . kalman方法推导2
1. 导论
1960年, R.E. Kalman发表了他使用迭代方法解决离散线性滤波问题的文章(《A New Approach to Linear Filtering and Prediction Problems》),揭开了卡尔曼滤波的历史篇章,得益于数字计算能力的发展,卡尔曼滤波方法在得到了广泛应用,尤其是在自主/辅助导航方面。 现在该方法在计算机图像处理,多数据融合方面也得到广泛应用。
1)第一个问题:什么是卡尔曼滤波
卡尔曼滤波是一种最优化自回归数据处理算法。其中最优化体现在:1)动态使用系统和测量设备的信息;2)该理论包含了系统噪声、测量误差、动态模型的不确定性;3)和感兴趣变量初始状态相关的各种可用信息。其中的自回归表现在:卡尔曼滤波器不需要使用/储存过去所有时刻的数据,使得卡尔曼滤波器计算性能好。
2)第二个问题:为什么用卡尔曼滤波
卡尔曼滤波最初是为了解决控制问题。通常在做系统分析或者控制系统设计的时候,研究人员都会期望从变量的内在联系来建立一个理论模型,通过对理论模型的研究来研究问题。但是,理论模型并不完美,而且模型的结果也只是近似罢了,而观测到的数据也不是完整和完美的。基于这些存在的问题,所以研究人员探索能够解决这些问题的方法,也就有了卡尔曼滤波。
2. 卡尔曼滤波方法推导
本方法注重思路,详细过程请参考文章开始列出的文献。
2.1 lossfunction
前边提到,卡尔曼是一种线性二次估计,相当于一种优化算法,优化算法的目标就是针对损失函数,并使得损失函数最大或者最小。首先考虑一个最简单的线性模型:
- y k = a k x k + n k y_k = a_kx_k + n_k yk=akxk+nk
其中 x k x_k xk是系统的状态量,但是这个状态量一般我们建模不准确,会存在一个误差,量化这个误差就使用如下形式: - e k = x k − x k ^ e_k = x_k - \hat{x_k} ek=xk−xk^ --这个误差越小,建模状态量越准确,得到的输出值也就越准确
但是 e k e_k ek有正有负不好优化,所以引入均方差(mean squared error, MSE), 可以只向一个方向优化: - l o s s f u n c i t o n = E ( e k 2 ) lossfunciton = E(e_k^2) lossfunciton=E(ek2)
2.2. 极大似然估计
原理:从已知结果出发,反推参数值,这个参数值将使得结果出现的概率最大。
抽象一下问题:已知观测结果y,需要反推状态参数 x k ^ \hat{x_k} xk^。所以最大似然估计的目标就是使得y的条件概率最大:
-
m a x ( P [ y ∣ x ^ ] ) max(P[y|\hat x]) max(P[y∣x^])
假设的随机噪声是符合标准偏差为$\sigma _k $的高斯分布(正态分布),那么条件概率可以写为: -
P ( y k ∣ x ^ k ) = K k e x p − ( ( y k − a k x ^ k ) 2 2 σ k 2 ) P(y_k|\hat x_k) = K_kexp-(\frac{(y_k -a_k\hat x_k)^2}{2\sigma _k^2}) P(yk∣x^k)=Kkexp−(2σk2(yk−akx^k)2)
那么极大似然概率就是取得乘积: -
P ( y k ∣ x ^ k ) = Π k K k e x p − ( ( y k − a k x ^ k ) 2 2 σ k 2 ) P(y_k|\hat x_k) =\mathop{\Pi} \limits_{k} K_kexp-(\frac{(y_k -a_k\hat x_k)^2}{2\sigma _k^2}) P(yk∣x^k)=kΠKkexp−(2σk2(yk−akx^k)2)
有exp, 可以通过取对数,将乘积转换为加和运算: -
l o g P ( y k ∣ x ^ k ) = − 1 2 ∑ k ( ( y k − a k x ^ k ) 2 2 σ k 2 ) + c o n s t a n t logP(y_k|\hat x_k) = -\frac{1}{2}\sum \limits_{k} (\frac{(y_k -a_k\hat x_k)^2}{2\sigma _k^2}) + constant logP(yk∣x^k)=−21k∑(2σk2(yk−akx^k)2)+constant
所以目标就是,找到 x ^ k \hat x_k x^k使得 l o g P ( y k ∣ x ^ k ) logP(y_k|\hat x_k) logP(yk∣x^k)最大,那么 x ^ k \hat x_k x^k就是最有可能的状态值。可以看到 l o g P ( y k ∣ x ^ k ) logP(y_k|\hat x_k) logP(yk∣x^k)的表达式其实就是2.1中描述的均方差的形式。我们可以通过最小化均方误差来求 x ^ k \hat x_k x^k。
2.3 状态方程推导
总体思路还是比较简单:由状态构建一个误差表达式,并将该表达式构建为MSE形式,根据已知条件推导,将MES改写称为一个包含已知量的表达式,求导,令导数等于零求得极小点,将该点回代至表达式,结束!整个推导过程的步骤也比较简单,只是涉及的量比较多,以及矩阵表达的形式,看起来比较繁杂而已。
已知条件,6个
一般情况下,我们将一个系统描述为状态方程和观测方程:
- 状态方程: x k + 1 = Φ x k + w k x_{k+1} = \Phi x_k + w_k xk+1=Φxk+wk
- 观测方程: z k = H x k + v k z_k = Hx_k + v_k zk=Hxk+vk
w k w_k wk为已知协方差的过程白噪声, v k v_k vk为已知协方差的测量白噪声,且二者不相关。
所以,以下两个值已知:
- Q = E ( w k w k T ) Q = E(w_kw_k^T) Q=E(wkwkT)
- R = E ( v k v k T ) R = E(v_kv_k^T) R=E(vkvkT)
- e k = x k − x ^ k e_k = x_k - \hat x_k ek=xk−x^k
- x ^ k = x ^ k ′ + K k ( z k − H x ^ k ′ ) \hat x_k =\hat x_k^{'} + K_k(z_k - H\hat x_k^{'}) x^k=x^k′+Kk(zk−Hx^k′) … x ^ k ′ \ldots \hat x_k^{'} …x^k′表示直接估计的值,该式将在下一节的极大似然估计中推导。
求解问题
优化的目标根据2.1就是:
l o s s f u n c i t o n = E ( e k 2 ) = E ( e k e k T ) = P k lossfunciton = E(e_k^2) = E(e_ke_k^T) = P_k lossfunciton=E(ek2)=E(ekekT)=Pk 一切都是从这个方程出发
推导过程
先处理已知条件:
- z k z_k zk 代入表达式: x ^ k = x ^ k ′ + K k ( z k − H x ^ k ) = x ^ k ′ + K k ( H x k + v k − H x ^ k ′ ) = K k H x k + ( I − K k H ) x k ′ + K k v k \hat x_k =\hat x_k^{'} + K_k(z_k - H\hat x_k) = \hat x_k^{'} + K_k(Hx_k + v_k - H\hat x_k^{'}) =K_kHx_k + (I - K_kH)x_k^{'} +K_kv_k x^k=x^k′+Kk(zk−Hx^k)=x^k′+Kk(Hxk+vk−Hx^k′)=KkHxk+(I−KkH)xk′+Kkvk
- x ^ k \hat x_k x^k 代入表达式: e k = x k − x ^ k = x k − ( K k H x k + ( I − K k H ) x k ′ + K k v k ) = ( I − K k H ) ( x k − x k ′ ) − K k v k e_k = x_k - \hat x_k =x_k -(K_kHx_k + (I - K_kH)x_k^{'} +K_kv_k) = (I - K_kH)(x_k - x_k^{'}) - K_kv_k ek<