非线性卡尔曼滤波及可观测性、观测度

小鱼儿 2022-09-11 01:11 225阅读 0赞

## 一、非线性系统的滤波方法 ##

对于非线性系统，一种常见的解决思路是先进行泰勒级数展开，略去高阶项后近似为线性系统，再做线性Kalman滤波估计。这种处理非线性系统的Kalman滤波方法称为扩展Kalman滤波(EKF)。

### 1、雅可比矩阵： ###

在向量微积分中，雅可比矩阵是一阶偏导数以一定方式排列成的矩阵，其行列式称为雅可比行列式。雅可比矩阵的重要性在于它体现了一个可微方程与给出点的最优线性逼近。因此，雅可比矩阵类似于多元函数的导数。

![watermark_type_ZHJvaWRzYW5zZmFsbGJhY2s_shadow_50_text_Q1NETiBA5p6v6I2j5pyJ5bi4_size_11_color_FFFFFF_t_70_g_se_x_16][]

从n维空间映射到m维空间，即状态量为n维，函数个数为m维；则雅可比矩阵为：![m\\times n][m_times n]

[雅可比矩阵][Link 1]

### 2、海森矩阵 ###

黑塞矩阵（Hessian Matrix），又译作海森矩阵、海瑟矩阵、海塞矩阵等，是一个多元函数的二阶偏导数构成的方阵，描述了函数的局部曲率。

黑塞矩阵常用于牛顿法解决优化问题，利用黑塞矩阵可判定多元函数的极值问题。  
在工程实际问题的优化设计中，所列的目标函数往往很复杂，为了使问题简化，常常将目标函数在某点邻域展开成泰勒多项式来逼近原函数，此时函数在某点泰勒展开式的矩阵形式中会涉及到黑塞矩阵。

![watermark_type_ZHJvaWRzYW5zZmFsbGJhY2s_shadow_50_text_Q1NETiBA5p6v6I2j5pyJ5bi4_size_20_color_FFFFFF_t_70_g_se_x_16][]

[海森矩阵][Link 2]

### 3、EKF滤波 ###

离散时间状态空间模型：

![20210911120933619.png][]

其中：

![X\_\{k\}][X_k]是![n][]维状态向量，;

![f(X\_\{k\})][f_X_k]是![n][]维非线性向量函数；

![Z\_\{k\}][Z_k]是![m][]维量测向量；

![h(X\_\{k\})][h_X_k]是![m][]维非线性向量函数；

\--%%--%%--%%--%%--%%--%%--%%--%%--%%--%%--%%--%%--%%--%%--%%--%%--%%--

若已知![k-1][]时刻状态![X\_\{K-1\}][X_K-1]的一个参考值/初始值(泰勒展开的初始值)记为：![X\_\{k-1\}^\{n\}][X_k-1_n]；

其中，真实值与参考值/初始值的偏差为：

![\\delta X\_\{k-1\}=X\_\{k-1\}-X\_\{k-1\}^\{n\}][delta X_k-1_X_k-1_-X_k-1_n]；

<1>通过EKF状态方程对![k][]时刻进行预测，可得：

![X\_\{k/k-1\}^\{n\}=f(X\_\{k-1\}^\{n\})][X_k_k-1_n_f_X_k-1_n]:

状态预测的偏差为：

![\\delta X\_\{k\}=X\_\{k\}-X\_\{k/k-1\}^\{n\}][delta X_k_X_k_-X_k_k-1_n]

<2>通过EKF量测方程和状态预测值![X\_\{k/k-1\}^\{n\}][X_k_k-1_n]对量测进行预测：

![Z\_\{k/k-1\}^\{n\}=h(X\_\{k/k-1\}^\{n\})][Z_k_k-1_n_h_X_k_k-1_n]

量测预测的偏差为：

![\\delta Z\_\{k\}=Z\_\{k\}-Z\_\{k/k-1\}^\{n\}][delta Z_k_Z_k_-Z_k_k-1_n]

<3>将偏差量![\\delta X\_\{k\}][delta X_k]和![\\delta X\_\{k-1\}][delta X_k-1]当作新的状态，且将![\\delta Z\_\{k\}][delta Z_k]当作新的量测，则可构成一个新的系统，并且是线性的；

![watermark_type_ZHJvaWRzYW5zZmFsbGJhY2s_shadow_50_text_Q1NETiBA5p6v6I2j5pyJ5bi4_size_11_color_FFFFFF_t_70_g_se_x_16 1][]

则可直接用标准线性kalman滤波方法进行偏差状态估计，公式如下：

![watermark_type_ZHJvaWRzYW5zZmFsbGJhY2s_shadow_50_text_Q1NETiBA5p6v6I2j5pyJ5bi4_size_16_color_FFFFFF_t_70_g_se_x_16][]

## 二、随机系统的可观、可控及稳定性和可观测度 ##

### 1、可控、可观和稳定性 ###

一维线性定常系统：

![20210911145918794.png][]

其中，![W\_\{k\}][W_k]、![V\_\{k\}][V_k]均为零均值白噪声，方差分别为![Q\\geq 0][Q_geq 0]、![R> 0][R_ 0]，且两者不想关，根据kalman滤波方程：

![watermark_type_ZHJvaWRzYW5zZmFsbGJhY2s_shadow_50_text_Q1NETiBA5p6v6I2j5pyJ5bi4_size_20_color_FFFFFF_t_70_g_se_x_16 1][]

### 2、状态估计的误差分配和可观测度分析 ###

<1>误差分配

当系统模型参数![\\Phi \_\{k/k-1\}][Phi _k_k-1]、![\\Gamma \_\{k-1\}][Gamma _k-1]、![H\_\{k\}][H_k]确定后，标准kalman滤波的均方误差阵![P\_\{k\}][P_k]仅与![P\_\{0\}][P_0]、![Q\_\{k\}][Q_k]、![R\_\{k\}][R_k]有关，而与状态初值![X\_\{0\}][X_0]及量测![Z\_\{k\}][Z_k]无关。

因此，状态估计误差分配问题是：![P\_\{0\}][P_0]、![Q\_\{k\}][Q_k]、![R\_\{k\}][R_k]对![P\_\{k\}][P_k]的影响。

kalman滤波均方误差阵的预测及量测更新公式重写如下：

![watermark_type_ZHJvaWRzYW5zZmFsbGJhY2s_shadow_50_text_Q1NETiBA5p6v6I2j5pyJ5bi4_size_18_color_FFFFFF_t_70_g_se_x_16][]

将上述两式中，第一个式带入第二个式子，可得：

![watermark_type_ZHJvaWRzYW5zZmFsbGJhY2s_shadow_50_text_Q1NETiBA5p6v6I2j5pyJ5bi4_size_20_color_FFFFFF_t_70_g_se_x_16 2][]

最终得：

![watermark_type_ZHJvaWRzYW5zZmFsbGJhY2s_shadow_50_text_Q1NETiBA5p6v6I2j5pyJ5bi4_size_20_color_FFFFFF_t_70_g_se_x_16 3][]

kalman滤波咋黄台估计的误差分配列表：

![watermark_type_ZHJvaWRzYW5zZmFsbGJhY2s_shadow_50_text_Q1NETiBA5p6v6I2j5pyJ5bi4_size_20_color_FFFFFF_t_70_g_se_x_16 4][]

<2>可观测度分析

如果系统建模准确，Kalman滤波均方误差阵![P\_\{k\}][P_k]反映了各状态之间的协方差，其中对角线元素为对应状态分量的估计均方误差。

因此，从![P\_\{k\}][P_k]随时间的变化过程中可以看出状态估计误差的变化情况，对角线元素的变化幅度，正好定量描述了对应状态分量估计效果的强弱程度。

针对状态向量中的每一个分量![X\_\{k(j)\}][X_k_j] （j=1,2,…，n），定义它的可观测度如下：

![20210911161145110.png][]

可观测度是针对某一状态分量在某一时刻而言的，其含义是某一状态分量的0时刻初始设置误差的标准差与同一状态分量在k时刻的滤波误差标准差的比值。

可观测度为无因次数量，在数值上越大，表明在经过Kalman滤波后，相应状态分量的估计误差下降程度越显著，或者说精度提升效果就越明显。

\--%%--%%--%%--%%--%%--%%--%%--%%--%%--%%--%%--%%--%%--%%--%%--%%--%%--

根据经验，可人为设置如下阈值大致判断状态分量![X\_\{k(j)\}][X_k_j]的可观测度强弱：

![2021091116262262.png][]

两种绘制曲线方式：

<1>绘制可观测度![\\delta \_\{k(j)\}][delta _k_j]相对于时间![k][]的对数曲线![k - log(\\sigma \_\{k(j)\})][k - log_sigma _k_j]，则能够直观地显示出状态估计地可观测度变化趋势；

<2>绘制出各状态分量误差的标准差曲线![k - sqrt(P\_\{k(jj)\})][k - sqrt_P_k_jj]，能够表示出状态估计误差的绝对大小变化情况；

\--%%--%%--%%--%%--%%--%%--%%--%%--%%--%%--%%--%%--%%--%%--%%--%%--%%--

在实际应用中， 如果针对某一问题， 设计了全面描述系统动力学行为的复杂高维滤波系统，**可观测度分析和前述误差分配技术**可应用于滤波器的优化：  
（1）对于不可观测的状态分量， 宜将其删去， 从而降低系统状态维数，减小滤波计算量；  
（2）对于可观测度较弱的状态分量， 与其对应的均方误差阵对角元素的初值不宜设置得太大，否则，对应的均方误差阵设置过大容易引起滤波过程中的估计误差剧烈波动， 在间接滤波中应当慎重使用反馈校正， 反馈时机不当有容易引起滤波发散；  
（3）对于可观测度较强的状态分量， 与其对应的均方误差阵对角元素可以设置成较大的初值，即便如此， 在随后滤波过程中也会快速减小收敛；  
（4）通过误差分配表，针对可观测的状态分量， 采取恰当措施合理地减小最大误差因素所占的比重，往往能够有效减小相应状态分量的滤波估计误差，即提高滤波估计精度。

[watermark_type_ZHJvaWRzYW5zZmFsbGJhY2s_shadow_50_text_Q1NETiBA5p6v6I2j5pyJ5bi4_size_11_color_FFFFFF_t_70_g_se_x_16]: /images/20220829/bdd886a616e74d45952551aa17e19ae1.png
[m_times n]: https://latex.codecogs.com/gif.latex?m%5Ctimes%20n
[Link 1]: https://baike.baidu.com/item/%E9%9B%85%E5%8F%AF%E6%AF%94%E7%9F%A9%E9%98%B5/10753754
[watermark_type_ZHJvaWRzYW5zZmFsbGJhY2s_shadow_50_text_Q1NETiBA5p6v6I2j5pyJ5bi4_size_20_color_FFFFFF_t_70_g_se_x_16]: /images/20220829/499154d4dbc94302a91992a9754360b0.png
[Link 2]: https://baike.baidu.com/item/%E9%BB%91%E5%A1%9E%E7%9F%A9%E9%98%B5/2248782
[20210911120933619.png]: /images/20220829/5e6a34b5f5c0489dbf0e2d5cf2a11c2c.png
[X_k]: https://latex.codecogs.com/gif.latex?X_%7Bk%7D
[n]: https://latex.codecogs.com/gif.latex?n
[f_X_k]: https://latex.codecogs.com/gif.latex?f%28X_%7Bk%7D%29
[Z_k]: https://latex.codecogs.com/gif.latex?Z_%7Bk%7D
[m]: https://latex.codecogs.com/gif.latex?m
[h_X_k]: https://latex.codecogs.com/gif.latex?h%28X_%7Bk%7D%29
[k-1]: https://latex.codecogs.com/gif.latex?k-1
[X_K-1]: https://latex.codecogs.com/gif.latex?X_%7BK-1%7D
[X_k-1_n]: https://latex.codecogs.com/gif.latex?X_%7Bk-1%7D%5E%7Bn%7D
[delta X_k-1_X_k-1_-X_k-1_n]: https://latex.codecogs.com/gif.latex?%5Cdelta%20X_%7Bk-1%7D%3DX_%7Bk-1%7D-X_%7Bk-1%7D%5E%7Bn%7D
[k]: https://latex.codecogs.com/gif.latex?k
[X_k_k-1_n_f_X_k-1_n]: https://latex.codecogs.com/gif.latex?X_%7Bk/k-1%7D%5E%7Bn%7D%3Df%28X_%7Bk-1%7D%5E%7Bn%7D%29
[delta X_k_X_k_-X_k_k-1_n]: https://latex.codecogs.com/gif.latex?%5Cdelta%20X_%7Bk%7D%3DX_%7Bk%7D-X_%7Bk/k-1%7D%5E%7Bn%7D
[X_k_k-1_n]: https://latex.codecogs.com/gif.latex?X_%7Bk/k-1%7D%5E%7Bn%7D
[Z_k_k-1_n_h_X_k_k-1_n]: https://latex.codecogs.com/gif.latex?Z_%7Bk/k-1%7D%5E%7Bn%7D%3Dh%28X_%7Bk/k-1%7D%5E%7Bn%7D%29
[delta Z_k_Z_k_-Z_k_k-1_n]: https://latex.codecogs.com/gif.latex?%5Cdelta%20Z_%7Bk%7D%3DZ_%7Bk%7D-Z_%7Bk/k-1%7D%5E%7Bn%7D
[delta X_k]: https://latex.codecogs.com/gif.latex?%5Cdelta%20X_%7Bk%7D
[delta X_k-1]: https://latex.codecogs.com/gif.latex?%5Cdelta%20X_%7Bk-1%7D
[delta Z_k]: https://latex.codecogs.com/gif.latex?%5Cdelta%20Z_%7Bk%7D
[watermark_type_ZHJvaWRzYW5zZmFsbGJhY2s_shadow_50_text_Q1NETiBA5p6v6I2j5pyJ5bi4_size_11_color_FFFFFF_t_70_g_se_x_16 1]: /images/20220829/05d4c971a0b449d8b942adecbafe7c5c.png
[watermark_type_ZHJvaWRzYW5zZmFsbGJhY2s_shadow_50_text_Q1NETiBA5p6v6I2j5pyJ5bi4_size_16_color_FFFFFF_t_70_g_se_x_16]: /images/20220829/d6fb8c4073734388b368897a8bbe692a.png
[20210911145918794.png]: /images/20220829/a5c3eca19b4a455abd4378da059e751d.png
[W_k]: https://latex.codecogs.com/gif.latex?W_%7Bk%7D
[V_k]: https://latex.codecogs.com/gif.latex?V_%7Bk%7D
[Q_geq 0]: https://latex.codecogs.com/gif.latex?Q%5Cgeq%200
[R_ 0]: https://latex.codecogs.com/gif.latex?R%3E%200
[watermark_type_ZHJvaWRzYW5zZmFsbGJhY2s_shadow_50_text_Q1NETiBA5p6v6I2j5pyJ5bi4_size_20_color_FFFFFF_t_70_g_se_x_16 1]: /images/20220829/21b84a18c86a4ac9b70aa7cedb2f2485.png
[Phi _k_k-1]: https://latex.codecogs.com/gif.latex?%5CPhi%20_%7Bk/k-1%7D
[Gamma _k-1]: https://latex.codecogs.com/gif.latex?%5CGamma%20_%7Bk-1%7D
[H_k]: https://latex.codecogs.com/gif.latex?H_%7Bk%7D
[P_k]: https://latex.codecogs.com/gif.latex?P_%7Bk%7D
[P_0]: https://latex.codecogs.com/gif.latex?P_%7B0%7D
[Q_k]: https://latex.codecogs.com/gif.latex?Q_%7Bk%7D
[R_k]: https://latex.codecogs.com/gif.latex?R_%7Bk%7D
[X_0]: https://latex.codecogs.com/gif.latex?X_%7B0%7D
[watermark_type_ZHJvaWRzYW5zZmFsbGJhY2s_shadow_50_text_Q1NETiBA5p6v6I2j5pyJ5bi4_size_18_color_FFFFFF_t_70_g_se_x_16]: /images/20220829/8a0e2c715ff9429a9e5fbb856e47cb7f.png
[watermark_type_ZHJvaWRzYW5zZmFsbGJhY2s_shadow_50_text_Q1NETiBA5p6v6I2j5pyJ5bi4_size_20_color_FFFFFF_t_70_g_se_x_16 2]: /images/20220829/0b588a162e8d4646bf2d3df19686ff2e.png
[watermark_type_ZHJvaWRzYW5zZmFsbGJhY2s_shadow_50_text_Q1NETiBA5p6v6I2j5pyJ5bi4_size_20_color_FFFFFF_t_70_g_se_x_16 3]: /images/20220829/47db8d414a154fc993db011a3c72b67c.png
[watermark_type_ZHJvaWRzYW5zZmFsbGJhY2s_shadow_50_text_Q1NETiBA5p6v6I2j5pyJ5bi4_size_20_color_FFFFFF_t_70_g_se_x_16 4]: /images/20220829/bda6db9a192d451c8e507f4d90a974d9.png
[X_k_j]: https://latex.codecogs.com/gif.latex?X_%7Bk%28j%29%7D
[20210911161145110.png]: /images/20220829/9b3aaf856eb742e79b179f108e3851a8.png
[2021091116262262.png]: /images/20220829/d51d028a28bd4b9d8a8835dff07636e1.png
[delta _k_j]: https://latex.codecogs.com/gif.latex?%5Cdelta%20_%7Bk%28j%29%7D
[k - log_sigma _k_j]: https://latex.codecogs.com/gif.latex?k%20-%20log%28%5Csigma%20_%7Bk%28j%29%7D%29
[k - sqrt_P_k_jj]: https://latex.codecogs.com/gif.latex?k%20-%20sqrt%28P_%7Bk%28jj%29%7D%29