最优估计与卡尔曼滤波基本原理

不念不忘少年蓝@ 2022-09-10 15:22 40阅读 0赞

## 一、最优估计 ##

### 1、估计理论： ###

估计理论是概率论与数理统计的一个分支，是根据受扰动的观测数据来提取系统某些参数或状态的一种数学方法。

定义估计量![\\breve\{X\}][breve_X]的误差为：

![20210910092723902.png][]

对于随机变量，常用均方误差(Mean Square Error,MSE)来衡量估计量与被估计量之间的误差大小，类似的，对于随机向量定义均方误差阵为：

![watermark_type_ZHJvaWRzYW5zZmFsbGJhY2s_shadow_50_text_Q1NETiBA5p6v6I2j5pyJ5bi4_size_20_color_FFFFFF_t_70_g_se_x_16][]

特别地，如果估计![\\breve\{X\}][breve_X]是无偏的，则均方误差阵就等于其误差的方差阵，即有![MSE\[\\breve\{X\}\]=var\[\\breve\{X\}\]][MSE_breve_X_var_breve_X]成立。一般也称标量函数![E\[\\breve\{X^\{T\}\\breve\{X\}\}\]][E_breve_X_T_breve_X]为均方误差，显然有：

![watermark_type_ZHJvaWRzYW5zZmFsbGJhY2s_shadow_50_text_Q1NETiBA5p6v6I2j5pyJ5bi4_size_15_color_FFFFFF_t_70_g_se_x_16][]

对两个均方误差阵进行大小比较，通常是指比较均方误差阵之迹的大小，它等价于比较均方误差的大小。

## 二、卡尔曼滤波方程 ##

### 1、kalman方程： ###

**<1>随机系统状态空间模型：**

![watermark_type_ZHJvaWRzYW5zZmFsbGJhY2s_shadow_50_text_Q1NETiBA5p6v6I2j5pyJ5bi4_size_11_color_FFFFFF_t_70_g_se_x_16][]

其中：

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

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

![W\_\{k-1\}][W_k-1]是![l][]维的系统噪声向量；

![V\_\{k\}][V_k]是![m][]维的量测噪声向量；

![\\Phi \_\{k/k-1\}][Phi _k_k-1]是![n][]维的状态一步转移矩阵；

![\\Gamma \_\{k/k-1\}][Gamma _k_k-1]是![n\\times l][n_times l]维的系统噪声分配矩阵；

![H\_\{k\}][H_k]是![m\\times n][m_times n]维的量测矩阵；

**<2>kalman滤波的五个公式：**

Ⅰ状态一步预测：

![20210910094935515.png][]

Ⅱ 状态一步预测均方误差阵：

![watermark_type_ZHJvaWRzYW5zZmFsbGJhY2s_shadow_50_text_Q1NETiBA5p6v6I2j5pyJ5bi4_size_14_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 1][]  
Ⅳ状态估计：

![watermark_type_ZHJvaWRzYW5zZmFsbGJhY2s_shadow_50_text_Q1NETiBA5p6v6I2j5pyJ5bi4_size_13_color_FFFFFF_t_70_g_se_x_16][]

Ⅴ状态估计均方误差阵：

![20210910095104165.png][]

注：

Ⅰ：量测一步预测均方误差阵![P\_\{ZZ k/k-1\}][P_ZZ k_k-1]

![watermark_type_ZHJvaWRzYW5zZmFsbGJhY2s_shadow_50_text_Q1NETiBA5p6v6I2j5pyJ5bi4_size_19_color_FFFFFF_t_70_g_se_x_16][]

Ⅱ：状态一步预测与量测一步预测之间的协均方误差阵![P\_\{XZ k/k-1\}][P_XZ k_k-1]

![watermark_type_ZHJvaWRzYW5zZmFsbGJhY2s_shadow_50_text_Q1NETiBA5p6v6I2j5pyJ5bi4_size_15_color_FFFFFF_t_70_g_se_x_16 1][]

Ⅲ：滤波增益计算公式，等价：

![watermark_type_ZHJvaWRzYW5zZmFsbGJhY2s_shadow_50_text_Q1NETiBA5p6v6I2j5pyJ5bi4_size_14_color_FFFFFF_t_70_g_se_x_16 1][]

Ⅳ： K时刻，状态估计均方误差阵：

![watermark_type_ZHJvaWRzYW5zZmFsbGJhY2s_shadow_50_text_Q1NETiBA5p6v6I2j5pyJ5bi4_size_19_color_FFFFFF_t_70_g_se_x_16 1][]

### **2、滤波流程框图和滤波初值的选择：** ###

![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][]

量测信息![Z\_\{k\}][Z_k]是Kalman滤波的最主要输入，但对于时变系统而言，系统结构参数![\\Phi \_\{k/k-1\}][Phi _k_k-1]、![\\Gamma \_\{k/k-1\}][Gamma _k_k-1]、![H\_\{k\}][H_k]及噪声![Q\_\{k-1\}][Q_k-1]、![R\_\{k\}][R_k]中的全部或部分是时变的，也是为滤波算法的输入，需实时更新。除了状态估计![\\breve\{X\}][breve_X]外，状态估计均方误差阵![P\_\{k\}][P_k]也是Kalman滤波输出的重要组成部分，![P\_\{k\}][P_k]对评价状态估计的质量发挥着非常重要的作用。

tips:

一般将初始均方误差阵![P\_\{0\}][P_0]设置为对角矩阵，各对角线元素的平方根粗略反映了状态分量初值的不确定度。

(1)对于可观测性较强的状态分量，对应的状态初值和均方误差阵设置偏差容许适当大些， 它们随着滤波更新将会快速收敛，如果均方误差阵设置太小，则会使收敛速度变慢。

(2)对于可观测性较弱的状态， 对应的状态初值和均方误差阵应该设置尽量准确，如果均方误差阵设置过大， 容易引起状态估计的剧烈波动， 反之，如果均方误差阵设置过小，同样会使状态收敛速度变慢，这两种情况下均方误差阵都不宜用于评估相应状态估计的精度。

(3)对于不可观测的状态分量， 其状态估计及其均方误差阵不会随滤波更新而变化， 即不会有滤波效果。

### 3、连续时间随机系统的离散化 ###

**<1>系统方程**

连续时间线性随机系统（线性随机微分方程）：

![watermark_type_ZHJvaWRzYW5zZmFsbGJhY2s_shadow_50_text_Q1NETiBA5p6v6I2j5pyJ5bi4_size_11_color_FFFFFF_t_70_g_se_x_16 1][]

离散化如下1：

![watermark_type_ZHJvaWRzYW5zZmFsbGJhY2s_shadow_50_text_Q1NETiBA5p6v6I2j5pyJ5bi4_size_10_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 4][]

注意到![Q\_\{k\}=q(t\_\{k-1\})T\_\{s\}][Q_k_q_t_k-1_T_s]，它与等效噪声![\\eta \_\{k-1\}][eta _k-1]（![W\_\{k\}][W_k]或![X\_\{k\}][X_k]）方差的量纲单位是一致的；

离散化如下2：

![watermark_type_ZHJvaWRzYW5zZmFsbGJhY2s_shadow_50_text_Q1NETiBA5p6v6I2j5pyJ5bi4_size_11_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 5][]

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

几种典型的连续时间随机过程及其离散化结果：

（1）一阶马尔可夫过程

连续时间：

![20210910110154292.png][]

离散化：

![20210910110214892.png][]

其中：

![watermark_type_ZHJvaWRzYW5zZmFsbGJhY2s_shadow_50_text_Q1NETiBA5p6v6I2j5pyJ5bi4_size_13_color_FFFFFF_t_70_g_se_x_16 1][]

（2）二阶马尔可夫过程：

连续时间：

![watermark_type_ZHJvaWRzYW5zZmFsbGJhY2s_shadow_50_text_Q1NETiBA5p6v6I2j5pyJ5bi4_size_12_color_FFFFFF_t_70_g_se_x_16][]

离散化：

![watermark_type_ZHJvaWRzYW5zZmFsbGJhY2s_shadow_50_text_Q1NETiBA5p6v6I2j5pyJ5bi4_size_17_color_FFFFFF_t_70_g_se_x_16][]

其中：

![watermark_type_ZHJvaWRzYW5zZmFsbGJhY2s_shadow_50_text_Q1NETiBA5p6v6I2j5pyJ5bi4_size_14_color_FFFFFF_t_70_g_se_x_16 2][]

（3）随机游走：

连续时间：

![20210910110336377.png][]

离散化：

![20210910110346304.png][]

其中：

![watermark_type_ZHJvaWRzYW5zZmFsbGJhY2s_shadow_50_text_Q1NETiBA5p6v6I2j5pyJ5bi4_size_13_color_FFFFFF_t_70_g_se_x_16 2][]

（4）随机常值：

连续时间：

![20210910110435846.png][]

离散化：

![20210910110445876.png][]

**<2>量测方程：**

在实际应用中， 大多数系统的量测方程是以离散形式直接给出的， 无需再进行离散化处理。

值得一提的是， 如果在一定量测频率范围内量测噪声的方差大小基本不变，则在量测设备允许的情况下选用较高的量测频率对提高滤波估计精度是有益的；

另一方面，如果系统状态变化比较平缓， 为了减小量测更新频率和计算量，则可将相继多次量测作平均处理， 并相应减少量测噪声大小， 利用平均量测进行滤波量测更新与进行多次量测更新是基本等效的。

[breve_X]: https://latex.codecogs.com/gif.latex?%5Cbreve%7BX%7D
[20210910092723902.png]: /images/20220829/18c34e5457cc4266a932f459c09841a1.png
[watermark_type_ZHJvaWRzYW5zZmFsbGJhY2s_shadow_50_text_Q1NETiBA5p6v6I2j5pyJ5bi4_size_20_color_FFFFFF_t_70_g_se_x_16]: /images/20220829/207b5d23b13746cab3d027889cfdedba.png
[MSE_breve_X_var_breve_X]: https://latex.codecogs.com/gif.latex?MSE%5B%5Cbreve%7BX%7D%5D%3Dvar%5B%5Cbreve%7BX%7D%5D
[E_breve_X_T_breve_X]: https://latex.codecogs.com/gif.latex?E%5B%5Cbreve%7BX%5E%7BT%7D%5Cbreve%7BX%7D%7D%5D
[watermark_type_ZHJvaWRzYW5zZmFsbGJhY2s_shadow_50_text_Q1NETiBA5p6v6I2j5pyJ5bi4_size_15_color_FFFFFF_t_70_g_se_x_16]: /images/20220829/a65d0fd4e0214135af31c42d92f0e1f6.png
[watermark_type_ZHJvaWRzYW5zZmFsbGJhY2s_shadow_50_text_Q1NETiBA5p6v6I2j5pyJ5bi4_size_11_color_FFFFFF_t_70_g_se_x_16]: /images/20220829/68d07afe8c84472f8f7e9a2295008636.png
[X_k]: https://latex.codecogs.com/gif.latex?X_%7Bk%7D
[Z_k]: https://latex.codecogs.com/gif.latex?Z_%7Bk%7D
[W_k-1]: https://latex.codecogs.com/gif.latex?W_%7Bk-1%7D
[l]: https://latex.codecogs.com/gif.latex?l
[V_k]: https://latex.codecogs.com/gif.latex?V_%7Bk%7D
[m]: https://latex.codecogs.com/gif.latex?m
[Phi _k_k-1]: https://latex.codecogs.com/gif.latex?%5CPhi%20_%7Bk/k-1%7D
[n]: https://latex.codecogs.com/gif.latex?n
[Gamma _k_k-1]: https://latex.codecogs.com/gif.latex?%5CGamma%20_%7Bk/k-1%7D
[n_times l]: https://latex.codecogs.com/gif.latex?n%5Ctimes%20l
[H_k]: https://latex.codecogs.com/gif.latex?H_%7Bk%7D
[m_times n]: https://latex.codecogs.com/gif.latex?m%5Ctimes%20n
[20210910094935515.png]: /images/20220829/587bacd0ac774137a0d5ecee53c96ff8.png
[watermark_type_ZHJvaWRzYW5zZmFsbGJhY2s_shadow_50_text_Q1NETiBA5p6v6I2j5pyJ5bi4_size_14_color_FFFFFF_t_70_g_se_x_16]: /images/20220829/61970d3040ed47baad674ded090ad269.png
[watermark_type_ZHJvaWRzYW5zZmFsbGJhY2s_shadow_50_text_Q1NETiBA5p6v6I2j5pyJ5bi4_size_20_color_FFFFFF_t_70_g_se_x_16 1]: /images/20220829/f081af8c2f2143e791797a5b97228897.png
[watermark_type_ZHJvaWRzYW5zZmFsbGJhY2s_shadow_50_text_Q1NETiBA5p6v6I2j5pyJ5bi4_size_13_color_FFFFFF_t_70_g_se_x_16]: /images/20220829/6dbad6b7ffe64f2fbe804da50132711a.png
[20210910095104165.png]: /images/20220829/2ea527b62c2c44d5bc2356159a9ac745.png
[P_ZZ k_k-1]: https://latex.codecogs.com/gif.latex?P_%7BZZ%20k/k-1%7D
[watermark_type_ZHJvaWRzYW5zZmFsbGJhY2s_shadow_50_text_Q1NETiBA5p6v6I2j5pyJ5bi4_size_19_color_FFFFFF_t_70_g_se_x_16]: /images/20220829/6d39f6e5b9f84e44908bfe43e798c12a.png
[P_XZ k_k-1]: https://latex.codecogs.com/gif.latex?P_%7BXZ%20k/k-1%7D
[watermark_type_ZHJvaWRzYW5zZmFsbGJhY2s_shadow_50_text_Q1NETiBA5p6v6I2j5pyJ5bi4_size_15_color_FFFFFF_t_70_g_se_x_16 1]: /images/20220829/40147a7270054866ba15249d6e6444dc.png
[watermark_type_ZHJvaWRzYW5zZmFsbGJhY2s_shadow_50_text_Q1NETiBA5p6v6I2j5pyJ5bi4_size_14_color_FFFFFF_t_70_g_se_x_16 1]: /images/20220829/cdfab79105d1464d999530cbedce6172.png
[watermark_type_ZHJvaWRzYW5zZmFsbGJhY2s_shadow_50_text_Q1NETiBA5p6v6I2j5pyJ5bi4_size_19_color_FFFFFF_t_70_g_se_x_16 1]: /images/20220829/b444a052e9d649b98d0a77fc6170ef9c.png
[watermark_type_ZHJvaWRzYW5zZmFsbGJhY2s_shadow_50_text_Q1NETiBA5p6v6I2j5pyJ5bi4_size_20_color_FFFFFF_t_70_g_se_x_16 2]: /images/20220829/39ef5d9be41b4653816be1e356d12c19.png
[watermark_type_ZHJvaWRzYW5zZmFsbGJhY2s_shadow_50_text_Q1NETiBA5p6v6I2j5pyJ5bi4_size_20_color_FFFFFF_t_70_g_se_x_16 3]: /images/20220829/72aca752fb7341d8a6c7da7431974608.png
[Q_k-1]: https://latex.codecogs.com/gif.latex?Q_%7Bk-1%7D
[R_k]: https://latex.codecogs.com/gif.latex?R_%7Bk%7D
[P_k]: https://latex.codecogs.com/gif.latex?P_%7Bk%7D
[P_0]: https://latex.codecogs.com/gif.latex?P_%7B0%7D
[watermark_type_ZHJvaWRzYW5zZmFsbGJhY2s_shadow_50_text_Q1NETiBA5p6v6I2j5pyJ5bi4_size_11_color_FFFFFF_t_70_g_se_x_16 1]: /images/20220829/7121fe0be8d14c618b1c5b388d1a0643.png
[watermark_type_ZHJvaWRzYW5zZmFsbGJhY2s_shadow_50_text_Q1NETiBA5p6v6I2j5pyJ5bi4_size_10_color_FFFFFF_t_70_g_se_x_16]: /images/20220829/4f116deffb754b4c8744c0b686d0f7e0.png
[watermark_type_ZHJvaWRzYW5zZmFsbGJhY2s_shadow_50_text_Q1NETiBA5p6v6I2j5pyJ5bi4_size_20_color_FFFFFF_t_70_g_se_x_16 4]: /images/20220829/4b017b21f18f4989a92b3da7a5140cdf.png
[Q_k_q_t_k-1_T_s]: https://latex.codecogs.com/gif.latex?Q_%7Bk%7D%3Dq%28t_%7Bk-1%7D%29T_%7Bs%7D
[eta _k-1]: https://latex.codecogs.com/gif.latex?%5Ceta%20_%7Bk-1%7D
[W_k]: https://latex.codecogs.com/gif.latex?W_%7Bk%7D
[watermark_type_ZHJvaWRzYW5zZmFsbGJhY2s_shadow_50_text_Q1NETiBA5p6v6I2j5pyJ5bi4_size_11_color_FFFFFF_t_70_g_se_x_16 2]: /images/20220829/a354886182144c858e09c1bdaaea4532.png
[watermark_type_ZHJvaWRzYW5zZmFsbGJhY2s_shadow_50_text_Q1NETiBA5p6v6I2j5pyJ5bi4_size_20_color_FFFFFF_t_70_g_se_x_16 5]: /images/20220829/97efa6bebe714766a202413740feb600.png
[20210910110154292.png]: /images/20220829/6d55b428466d478fb563557b6a9a2b75.png
[20210910110214892.png]: /images/20220829/78e2c0f730f340bfab82e0232faf592e.png
[watermark_type_ZHJvaWRzYW5zZmFsbGJhY2s_shadow_50_text_Q1NETiBA5p6v6I2j5pyJ5bi4_size_13_color_FFFFFF_t_70_g_se_x_16 1]: /images/20220829/cb62be2720f74c988d5235114bc2758f.png
[watermark_type_ZHJvaWRzYW5zZmFsbGJhY2s_shadow_50_text_Q1NETiBA5p6v6I2j5pyJ5bi4_size_12_color_FFFFFF_t_70_g_se_x_16]: /images/20220829/bf3d4dc25d6e4e0a8141463f6faf4830.png
[watermark_type_ZHJvaWRzYW5zZmFsbGJhY2s_shadow_50_text_Q1NETiBA5p6v6I2j5pyJ5bi4_size_17_color_FFFFFF_t_70_g_se_x_16]: /images/20220829/7f010376684742e39729fdfa7c0e8191.png
[watermark_type_ZHJvaWRzYW5zZmFsbGJhY2s_shadow_50_text_Q1NETiBA5p6v6I2j5pyJ5bi4_size_14_color_FFFFFF_t_70_g_se_x_16 2]: /images/20220829/c1ae110b3d844989b67c03847460b870.png
[20210910110336377.png]: /images/20220829/18349b160a734afdb4cd2f4e31395f9c.png
[20210910110346304.png]: /images/20220829/517060a8b2d94a998657aeb2bd59ba10.png
[watermark_type_ZHJvaWRzYW5zZmFsbGJhY2s_shadow_50_text_Q1NETiBA5p6v6I2j5pyJ5bi4_size_13_color_FFFFFF_t_70_g_se_x_16 2]: /images/20220829/b5fc530bc5e049e2907ecdb62de2feaa.png
[20210910110435846.png]: /images/20220829/a3a2ae632ccc4a008ab09015b4fca0db.png
[20210910110445876.png]: /images/20220829/530fa26b060c4ca6962bd7936c118c5d.png