Kalman滤波器推导与实现(Python版本)

  • 卡尔曼滤波(Kalman Filter)

本文在参考文献的基础上添加了自己的理解,若有不当之处,敬请指正
卡尔曼滤波以前接触过,但是没有仔细推导,这次参考文献仔细推导实现,也是第一次完全的通过vscode+markdown来完成写作。
卡尔曼滤波,也被称为线性二次估计(Liner Quadratic Estimation, LQE),可以作为平滑数据、预测数据、滤波器;本人理解:是一个观察→预测的过程,我接触到的卡尔曼滤波主要有KF(卡尔曼滤波,线性),EKF(扩展卡尔曼滤波,非线性),UKF(无迹卡尔曼滤波,强非线性)。此处,只研究基本的KF。

在这里插入图片描述
     图1 来自老爷子的亲切凝视


  • 参考:

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=xkxk^   --这个误差越小,建模状态量越准确,得到的输出值也就越准确
    但是 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[yx^])
    假设的随机噪声是符合标准偏差为$\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(ykx^k)=Kkexp(2σk2(ykakx^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(ykx^k)=kΠKkexp(2σk2(ykakx^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(ykx^k)=21k(2σk2(ykakx^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(ykx^k)最大,那么 x ^ k \hat x_k x^k就是最有可能的状态值。可以看到 l o g P ( y k ∣ x ^ k ) logP(y_k|\hat x_k) logP(ykx^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=xkx^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(zkHx^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(zkHx^k)=x^k+Kk(Hxk+vkHx^k)=KkHxk+(IKkH)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<
评论 2
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值