详细总结卡尔曼滤波原理+具体案例分析

Mr_Winter
2,942 阅读6分钟

「这是我参与2022首次更文挑战的第5天，活动详情查看：2022首次更文挑战」

1 状态观测器

在工程上，部分状态参数测量的成本过高，或是用现有仪器无法测得，此时需要引入状态观测器。状态观测器以原系统输入、输出为输入，输出估计的状态变量。

原系统状态描述：

{xk=Axk−1+Buk−1yk=Cxk\begin{cases} x_k=Ax_{k-1}+Bu_{k-1}\\ y_k=Cx_k\\\end{cases}

状态观测器状态描述：

{x~k=Ax~k−1+Buk−1+f[yk−1−y~k−1]y~k=Cx~k\begin{cases} \tilde{x}_k=A\tilde{x}_{k-1}+Bu_{k-1}+f\left[ y_{k-1}-\tilde{y}_{k-1} \right]\\ \tilde{y}_k=C\tilde{x}_k\\\end{cases}

定义误差向量ek=xk−x~ke_k=x_k-\tilde{x}_k，则两系统状态方程相减得

系统误差传递方程ek=(A−fC)ek−1系统误差传递方程e_k=\left( A-fC \right) e_{k-1}

其通解为矩阵指数函数，所以当A−fCA-fC特征值小于0时，误差向量eke_k各分量均趋于0，即状态观测器能直接估计原系统状态。

本质上，状态观测器是针对状态空间方程描述的确定系统，从错误的系统状态估计值不断收敛到正确的系统状态估计值的数学模。

2 状态滤波器

实际的系统往往是随机过程，状态描述为：

{xk=Axk−1+Buk−1+wk−1yk=Cxk+vk\begin{cases} x_k=Ax_{k-1}+Bu_{k-1}+w_{k-1}\\ y_k=Cx_k+v_k\\\end{cases}

其中wkw_k称为过程噪声，主要由系统运行过程中的外力导致，如旋翼飞行器运动过程中受到的阵风；vkv_k称为测量噪声，主要由系统测量过程中测量仪器误差、精度不足导致。

本质上，状态滤波器是针对随机状态空间方程描述的随机系统，从不确定观测中提取信息，得到系统状态最优估计的数学模型。当wkw_k与vkv_k为高斯白噪声且相互独立时，状态滤波器为卡尔曼滤波器。

状态滤波器的核心是通过贝叶斯原理不断调整滤波器增益矩阵，以减小随机干扰，使估计的系统状态趋近于真实状态；而状态观测器的核心是通过极点配置确定观测器增益矩阵，使估计的状态跟踪当前系统的状态。

一般地，状态滤波器性能优于状态观测器，但在实际系统中，若随机噪声非高斯噪声，或有若干非线性环节，按常规噪声模型建立的状态滤波器可能发散，此时则更需要状态观测器的稳定性。

3 卡尔曼滤波器

机器学习：详解贝叶斯网络+例题分析曾提到贝叶斯方法的思考模式：

参数先验信息π(θ)+样本观测数据X=后验分布P(θ∣X)\text{参数先验信息}\pi \left( \theta \right) +\text{样本观测数据}X=\text{后验分布}P\left( \theta |X \right)

即在得到新的样本信息之前，人们对模型的认知是先验分布π(θ)\pi \left( \theta \right)，在得到新的样本信息XX后，人们对模型的认知为后验分布P(θ∣X)P\left( \theta |X \right)。

由此引出卡尔曼滤波器。考虑一个随机系统：

{xk=Axk−1+Buk−1+wk−1zk=Cxk+vk\begin{cases} x_k=Ax_{k-1}+Bu_{k-1}+w_{k-1}\\ z_k=Cx_k+v_k\\\end{cases}

其中wk N(0,Q)w_k~N\left( 0, Q \right)、vk N(0,R)v_k~N\left( 0, R \right)，且wkw_k与vkv_k相互独立。

忽略噪声可建立该系统的先验数学模型(噪声仅满足随机分布，无法建模)：

状态预测方程{x^k−=Ax^k−1+Buk−1z^k−=Cx^k{\text{状态预测方程}\begin{cases} \hat{x}_{k}^{-}=A\hat{x}_{k-1}+Bu_{k-1}\\ \hat{z}_{k}^{-}=C\hat{x}_k\\\end{cases}}

基于上述系统，从贝叶斯方法引出卡尔曼滤波器状态更新方程：

状态更新方程:x^k=x^k−+Kk(zk−z^k−){\text{状态更新方程}: \hat{x}_k=\hat{x}_{k}^{-}+K_k\left( z_k-\hat{z}_{k}^{-} \right) }

其中x^k\hat{x}_k是后验状态估计，即经过一次修正后，当前系统状态的最优估计值；x^k−\hat{x}_{k}^{-}为系统状态的先验估计；zkz_k为当前状态的测量样本值；z^k−\hat{z}_{k}^{-}为当前状态的先验测量值；zk−z^k−z_k-\hat{z}_{k}^{-}代表实际系统与估计系统间随机误差的影响；KkK_k权衡先验模型与实测数据间对后验分布的关系，称为卡尔曼增益，显然，zk−z^k−z_k-\hat{z}_{k}^{-}越小表明实际与预测越接近，则后验分布越趋近于先验分布，zk−z^k−z_k-\hat{z}_{k}^{-}越大表明预测越不可信，需要通过实测来大幅修正先验分布。

下面要确定KkK_k使后验模型的修正效果最佳，即x^k\hat{x}_k越接近真实值xk{x}_k。定义误差向量

{后验误差向量ek=xk−x^k先验误差向量ek−=xk−x^k−\begin{cases} \text{后验误差向量}e_k=x_k-\hat{x}_k\\ \text{先验误差向量}e_{k}^{-}=x_k-\hat{x}_{k}^{-}\\\end{cases}

定义后验误差协方差矩阵为:

Pk=E(ekekT)=[σe12σe1σe2⋯σe1σenσe2σe1σe22⋯σe2σen⋮⋮⋱⋮σenσe1σenσe2⋯σen2]P_k=E\left( e_ke_{k}^{T} \right) =\left[ \begin{matrix} \sigma _{e1}^{2}& \sigma _{e1}\sigma _{e2}& \cdots& \sigma _{e1}\sigma _{en}\\ \sigma _{e2}\sigma _{e1}& \sigma _{e2}^{2}& \cdots& \sigma _{e2}\sigma _{en}\\ \vdots& \vdots& \ddots& \vdots\\ \sigma _{en}\sigma _{e1}& \sigma _{en}\sigma _{e2}& \cdots& \sigma _{en}^{2}\\\end{matrix} \right]

同理也有先验误差协方差矩阵Pk−P_{k}^{-}。根据最小方差估计原理，设损失函数为tr(Pk)\mathrm{tr}\left( P_k \right)，即要求

Kk=argmin⁡[tr(Pk)]K_k=\mathrm{arg}\min \left[ \mathrm{tr}\left( P_k \right) \right]

将PkP_k展开为： 在这里插入图片描述 上面运用了状态更新方程与状态预测方程，考虑到ek−e_{k}^{-}与vkv_k相互独立，则进一步：

Pk=(I−KkC)E(ek−ek−T)(I−KkC)T+KkE(vkvkT)KkT=(I−KkC)Pk−(I−KkC)T+KkRKkTP_k=\left( I-K_kC \right) E\left( e_{k}^{-}{e_{k}^{-}}^T \right) \left( I-K_kC \right) ^T+K_kE\left( v_kv_{k}^{T} \right) K_{k}^{T}\\=\left( I-K_kC \right) P_{k}^{-}\left( I-K_kC \right) ^T+K_kRK_{k}^{T}

令∂tr(Pk)∂Kk=0\frac{\partial \mathrm{tr}\left( P_k \right)}{\partial K_k}=0，即

卡尔曼增益调整方程Kk=Pk−CTCPk−CT+R{\text{卡尔曼增益调整方程}K_k=\frac{P_{k}^{-}C^T}{CP_{k}^{-}C^T+R}}

将KkK_k代入后验误差协方差矩阵表达式，即得

协方差更新方程Pk=(I−KkH)Pk−{\text{协方差更新方程}P_k=\left( I-K_kH \right) P_{k}^{-}}

要更新KkK_k，则只需要确定先验误差协方差矩阵Pk−P_{k}^{-}。同样地，将 Pk−P_{k}^{-}展开为

Pk−=E(ek−ek−T)=E[(xk−x^k−)(xk−x^k−)T]=E[(Aek−1+wk−1)(Aek−1+wk−1)T]=AE(ek−1ek−1T)AT+E(wk−1wk−1T)P_{k}^{-}=E\left( e_{k}^{-}e_{k}^{-T} \right) \\=E\left[ \left( x_k-\hat{x}_{k}^{-} \right) \left( x_k-\hat{x}_{k}^{-} \right) ^T \right] \\=E\left[ \left( Ae_{k-1}+w_{k-1} \right) \left( Ae_{k-1}+w_{k-1} \right) ^T \right] \\=AE\left( e_{k-1}e_{k-1}^{T} \right) A^T+E\left( w_{k-1}w_{k-1}^{T} \right)

考虑到eke_k与wkw_k相互独立，进一步得到

协方差预测方程Pk−=APk−1AT+Q{\text{协方差预测方程}P_{k}^{-}=AP_{k-1}A^T+Q}

至此，得到卡尔曼滤波的五大基本公式，其中核心方程为基于最小方差估计的卡尔曼增益调整方程，具体工作流程如图所示。

image.png

4 具体案例：船舶GPS定位

有一船舶出港沿某直线方向航行，辅助北斗卫星进行定位和测速。假设 ① 船舶加速度a(t)a\left( t \right)=机动加速度u(t)u\left( t \right)+随机加速度w(t)w\left( t \right)，其中w(t)w\left( t \right)符合高斯分布； ② GPS观测噪声v(t)v\left( t \right)符合高斯分布。 要求用卡尔曼滤波器估计真实运动轨迹。

解决方案

首先建立随机系统的真实模型。以码头出发点为原点，采样周期(雷达扫描周期)为TT，用s(k)s(k)表示船舶在采样时刻kTkT处的真实位置，用z(k)z\left( k \right)表示在时刻kTkT处的GPS定位观测值，则真实系统输出方程为z(k)=s(k)+v(k)z(k)=s(k)+v(k)。

记在kTkT时刻处船舶速s˙(k)\dot{s}\left( k \right)，加速度为a(k)a\left( k \right)，由匀加速公式有：

s(k+1)=s(k)+s˙(k)T+12a(k)T2⇒s˙(k+1)=s˙(k)+Ta(k)s\left( k+1 \right) =s\left( k \right) +ṡ\left( k \right) T+\frac{1}{2}a\left( k \right) T^2\\\Rightarrow \dot{s}\left( k+1 \right) =ṡ\left( k \right) +Ta\left( k \right)

其中a(k)=u(k)+w(k)a(k)=u(k)+w(k)。

定义在采样时刻kTkT系统状态x(k)x\left( k \right)为船舶的位置和速度，即x(k)=[s(k)s˙(k)]Tx(k)=\left[ \begin{matrix} s(k)& \dot{s}(k)\\\end{matrix} \right] ^T可得到船舶运动的状态空间模型

{[s(k+1)s˙(k+1)]=[1T01][s(k)s˙(k)]+[0.5TT]u(k)+[0.5TT]w(k)z(k)=[10][s(k)s˙(k)]+v(k)\begin{cases} \left[ \begin{array}{c} s(k+1)\\ \dot{s}(k+1)\\\end{array} \right] =\left[ \begin{matrix} 1& T\\ 0& 1\\\end{matrix} \right] \left[ \begin{array}{c} s(k)\\ \dot{s}(k)\\\end{array} \right] +\left[ \begin{array}{c} 0.5T\\ T\\\end{array} \right] u(k)+\left[ \begin{array}{c} 0.5T\\ T\\\end{array} \right] w(k)\\ z(k)=\left[ \begin{matrix} 1& 0\\\end{matrix} \right] \left[ \begin{array}{c} s(k)\\ \dot{s}(k)\\\end{array} \right] +v(k)\\\end{cases}

在不考虑机动目标的动力因素即u(k)=0u(k)=0时，将匀速直线运动的船舶系统扩展到四维，状态包含水平和纵向的位置和速度，则系统方程可化为

[x(k)x˙(k)y(k)y˙(k)]=[1T000100001T0001][x(k−1)x˙(k−1)y(k−1)y˙(k−1)]+[0.5T2T0.5T2T][w1(k)w2(k)w3(k)w4(k)]z(k)=[10000010][x(k)x˙(k)y(k)y˙(k)]+[v1(k)v2(k)]\left[ \begin{array}{c} x(k)\\ \dot{x}(k)\\ y(k)\\ \dot{y}(k)\\\end{array} \right] =\left[ \begin{matrix} 1& T& 0& 0\\ 0& 1& 0& 0\\ 0& 0& 1& T\\ 0& 0& 0& 1\\\end{matrix} \right] \left[ \begin{array}{c} x(k-1)\\ \dot{x}(k-1)\\ y(k-1)\\ \dot{y}(k-1)\\\end{array} \right] +\left[ \begin{matrix} 0.5T^2& & & \\ & T& & \\ & & 0.5T^2& \\ & & & T\\\end{matrix} \right] \left[ \begin{array}{c} w_1\left( k \right)\\ w_2\left( k \right)\\ w_3\left( k \right)\\ w_4\left( k \right)\\\end{array} \right] \\z(k)=\left[ \begin{matrix} 1& 0& 0& 0\\ 0& 0& 1& 0\\\end{matrix} \right] \left[ \begin{array}{c} x(k)\\ \dot{x}(k)\\ y(k)\\ \dot{y}(k)\\\end{array} \right] +\left[ \begin{array}{c} v_1\left( k \right)\\ v_2\left( k \right)\\\end{array} \right]

现假设初始位置为(-100m，200m)，水平运动初速度为2m/s，垂直运动初速度为20m/s，雷达扫描周期T=1sT=1s，则系统状态空间进一步化为

{x(k+1)=Ax(k)+Bu(k)+Γw(k)z(k)=Hx(k)+v(k)\begin{cases} x(k+1)=Ax(k)+Bu(k)+\varGamma w(k)\\ z(k)=Hx(k)+v(k)\\\end{cases}

其中A=[1100010000110001]A=\left[ \begin{matrix} 1& 1& 0& 0\\ 0& 1& 0& 0\\ 0& 0& 1& 1\\ 0& 0& 0& 1\\\end{matrix} \right]，B=Γ=[0.510.51]B=\varGamma =\left[ \begin{matrix} 0.5& & & \\ & 1& & \\ & & 0.5& \\ & & & 1\\\end{matrix} \right],H=[10000010]H=\left[ \begin{matrix} 1& 0& 0& 0\\ 0& 0& 1& 0\\\end{matrix} \right] 。

设过程噪声w∼N(0,0.12)w\sim N(0,0.1^2)，其四个分量独立同分布，则

Q∗=E(w4×1w4×1T)=σw2[1111]Q^*=E(w_{4\times 1}w_{4\times 1}^{T})=\sigma _{w}^{2}\left[ \begin{matrix} 1& & & \\ & 1& & \\ & & 1& \\ & & & 1\\\end{matrix} \right]

考虑噪声矩阵Γ\varGamma的加权得到过程噪声协方差矩阵

Q=Q∗⋅Γ=σw2[0.510.51]Q=Q^*\cdot \varGamma =\sigma _{w}^{2}\left[ \begin{matrix} 0.5& & & \\ & 1& & \\ & & 0.5& \\ & & & 1\\\end{matrix} \right]

观测噪声v∼N(0,102)v\sim N(0,10^2)，其两个分量独立同分布，则观测噪声协方差矩阵

R=σv2[11]R=\sigma _{v}^{2}\left[ \begin{matrix} 1& \\ & 1\\\end{matrix} \right]

依此建立卡尔曼滤波器模型。

设置最优估计初值x^(1)=[−1002−20020]T\hat{x}(1)=\left[ \begin{matrix} -100& 2& -200& 20\\\end{matrix} \right] ^T、先验协方差矩阵P1−=[1000010000100001]P_{1}^{-}=\left[ \begin{matrix} 1& 0& 0& 0\\ 0& 1& 0& 0\\ 0& 0& 1& 0\\ 0& 0& 0& 1\\\end{matrix} \right]，并以此开始迭代，得到实验图像如图所示。

image.png

在使用卡尔曼滤波器时，应至少保证系统模型或系统测量至少一个有足够的精度，否则卡尔曼滤波器无法从中提取到正确的估计信息。这是因为卡尔曼滤波调整的是对先验模型值与仪器测量值间的信任权重，若二者都不准确则卡尔曼跟踪的数值亦不准确。为验证这一结论，下面保持测量噪声不变，调整过程噪声方差，如图所示。

image.png

完整代码私信获取。

avatar
文章
阅读
粉丝