夜话卡尔曼滤波(2) - 变量定义

1 序

       一晃梅花又开了，又是三九时节，真是梅花香自苦寒来。上一篇我们浮光掠影地了解了卡尔曼滤波的公式，并且体会了从公式到代码的过程。这个里面有一个悬疑的问题，就是怎么样结合自已的应用，定义这些变量或参数，它们的维度是多少，该如何取值等等。不急，一样一样来。

2.数据类型

       诸位看到公式里面的，有大小写两种类型的字母变量，大写一般是指矩阵，小写一般指向量。在使用卡尔曼滤波时，这两个概念是基本的数据类型，是绕不过去的。从程序的角度上看，我们可以把它看成数组对象，并不复杂。

2.1 矩阵

       矩阵表达m*n项数据，可以组织为一个[m,n]二维数组。

#例如3行4列矩阵
m = np.matrix([[11, 12, 13, 14, 15],
               [21, 22, 23, 24, 25],
               [31, 32, 33, 34, 35]])   
#  [[11 12 13 14 15]
#   [21 22 23 24 25]
#   [31 32 33 34 35]]

• 矩阵转置
  这个就是将数组中元素的进行位置转换，对于程序员来说，这个不是难事，上面无非就是数组元素位置互换的戏法，没有什么难度系数。矩阵转置一般是在矩阵变量右上角加一个T表示转置，比如矩阵M的转置表达为$M^T$。

#m在上一节有定义，而.T在代码中表示转置
print(m.T)
#  [[11 21 31]
#   [12 22 32]
#   [13 23 33]
#   [14 24 34]
#   [15 25 35]]

• 对角矩阵
  对角矩阵是一个从左上角到右下角的对角线（称为主对角线）之外的元素皆为0的矩阵。对角线上的元素可以为0或其它值。

#np.diag函数用于创建对角矩阵
#下面创建4*4的 对角矩阵
P = np.diag([1,2, 3, 4])
print(P)
#  [[1 0 0 0]
#   [0 2 0 0]
#   [0 0 3 0]
#   [0 0 0 4]]

• 单位矩阵
  有一种矩阵如同四则运算中乘法中的1，这种矩阵被称为单位矩阵。它是个方阵，主对角线上的元素均为1，除此以外全都为0。

#np.eye函数用于单位矩阵
#下面创建4*4的单位矩阵
I = np.eye(4)
print(I)
#  [[1. 0. 0. 0.]
#   [0. 1. 0. 0.]
#   [0. 0. 1. 0.]
#   [0. 0. 0. 1.]]

• 逆矩阵
  对于自然数，某个数与其倒数的乘积为1，即$x\ast \frac{1}{x}=1$。以矩阵视角来看，如果这个X是一个矩阵,那么式子后面的1便是上面所提到的单位矩阵，$\frac{1}{x}$便是X的逆矩阵，是不是有点像X的倒数？无独有偶，逆矩阵表达一般也是在矩阵变量右上角加一个-1表示求逆，比如矩阵M的逆矩阵表达为$M^{-1}$。

A = np.array([[1, 0, 0],
              [0, 6, 0],
              [0, 0, 9]])
R=np.linalg.inv(A) # 求逆矩阵
print('R=', R)  
print('A*R=', A*R) 
#  R= [[1.         0.         0.        ]
#      [0.         0.16666667 0.        ]
#      [0.         0.         0.11111111]]
# 还原看看，体会一下。下面乘积的结果就是一个单位矩阵
#  A*R= [[1. 0. 0.]
#        [0. 1. 0.]
#        [0. 0. 1.]] 

对于c语言，求逆矩阵可能代码要长一些，不过可以找到诸多的方法；应该在各种方法之间寻找一个时间和空间的平衡。

• 矩阵加减法
  矩阵加减无非就是对应的元素进行加减，很简单。
  $$\left|\begin{array}{ccc}1& 2& 3\\ 4& 5& 6\\ 7& 8& 9\end{array}\right|+\left|\begin{array}{ccc}11& 12& 13\\ 14& 15& 16\\ 17& 18& 19\end{array}\right|=\left|\begin{array}{ccc}1+11& 2+12& 3+13\\ 4+14& 5+15& 6+16\\ 7+17& 8+18& 9+19\end{array}\right|$$

L = np.matrix([[1, 2, 3 ],
               [4, 5, 6],
               [7, 8, 9]])  
R = np.matrix([[11, 12, 13 ],
               [14, 15, 16],
               [17, 18, 19]])
print(L+R)
#  [[12 14 16]
#   [18 20 22]
#   [24 26 28]]

• 矩阵乘法
  从下面的式子体会一下矩阵相乘的方法。
  $$\begin{gathered} \left|\begin{array}{ccc}1& 2& 3\\ 4& 5& 6\\ 7& 8& 9\end{array}\right|\left|\begin{array}{ccc}11& 12& 13\\ 14& 15& 16\\ 17& 18& 19\end{array}\right|= \\ \left|\begin{array}{ccc}1\ast 11+2\ast 14+3\ast 17& 1\ast 12+2\ast 15+3\ast 18& 1\ast 13+2\ast 16+3\ast 19\\ 4\ast 11+5\ast 14+6\ast 17& 4\ast 12+5\ast 15+6\ast 18& 4\ast 13+5\ast 16+6\ast 19\\ 7\ast 11+8\ast 14+9\ast 17& 7\ast 12+8\ast 15+9\ast 18& 7\ast 13+8\ast 16+9\ast 19\end{array}\right|=\left|\begin{array}{ccc}90& 96& 102\\ 216& 231& 246\\ 342& 366& 390\end{array}\right| \end{gathered}$$

#借用上面的变量哈
print(L*R)
#  [[ 90  96 102]
#   [216 231 246]
#   [342 366 390]]

2.2 向量

       向量是n行1列的数据，可以组织为一维[n]数组。在卡尔曼滤波中，虽然存在向量与矩阵的运算，但没有必要专门针对向量引入一种数据类型，而是将它视为n行1列的矩阵，是比较简单的做法。

经过.T转置，就变成了4行1列
x = np.matrix([[1, 2, 3, 4]]).T
print(x)
#  [[1]
#   [2]
#   [3]
#   [4]]

2.3 矩阵乘以向量

       虽然在程序中一般将向量视为1列的矩阵，矩阵与向量的乘法视同于矩阵相乘，但是卡尔曼滤波中有不少矩阵与向量的乘法，所以还是关注一下，以求清澈。从下面的式子体会一下矩阵乘以向量的方法。
$$\left|\begin{array}{cccc}1& 2& 3& 4\\ 5& 6& 7& 8\\ 9& 10& 11& 12\\ 13& 14& 15& 16\end{array}\right|\left(\begin{array}{c}3\\ 4\\ 5\\ 6\end{array}\right)=\left|\begin{array}{c}1\ast 3+2\ast 4+3\ast 5+4\ast 6\\ 5\ast 3+6\ast 4+7\ast 5+8\ast 6\\ 9\ast 3+10\ast 4+11\ast 5+12\ast 6\\ 13\ast 3+14\ast 4+15\ast 5+16\ast 6\end{array}\right|=\left|\begin{array}{c}50\\ 122\\ 194\\ 266\end{array}\right|$$

M = np.matrix([[1,2,3,4],
               [5,6,7,8],
               [9,10,11,12],
               [13,14,15,16],])  
V = np.matrix([[3, 4, 5, 6]]).T
print(M*V)
#  [[ 50]
#   [122]
#   [194]
#   [266]]

       在这里，关键要记住矩阵和向量相乘得到一个新的向量。

3 数据维度

       在使用卡尔曼滤波之前，需要静下心神，根据应用的要求，确定公式中变量的特性维数(公式中变量不是向量就是矩阵，记得不），这个也算是建模过程吧。和上一篇一样，在第一个公式中$B$和$u$在本次讨论中忽略，因为这两个不太常用，先不考虑它。
       下面分析一下公式中相关的变量。方便起见，我们仍以小车二维定位为例说明问题，为了避免与常见卡尔曼公式字母冲突产生岐义，采用$ab$表达平面中的横、纵坐标。

3.1 测量向量z

       在这里我们有2个数据来源项目，即a/b表达的横纵坐标，测量向量z的维数取决于数据来源的数量，显然这里为2。即测量向量$z=(a,b{)}^T$。在这个例子中，z向量的维数是2，记为z_DIM=2。

a=12.06
b=34.07
z = np.matrix([[a,b]]).T
print(z)
print(x)
#  [[12.06]
#   [34.07]]

3.2 状态向量x

       状态向量表达小车的当前状态，肯定应该包括横纵坐标a/b；因为小车是移动的，移动速度也是隐含的状态之一， 加入隐含的状态，是为了更好地进行状态预测。为简单起见，暂不考虑加速度，只是将速度分为a/b两个方向，由于采样频率固定，可以采用增量值来表示速度，则状态向量$x=(a,b,\Delta a,\Delta b{)}^T$。在这个例子中，状态向量x的维数是4，记为x_DIM=4。

a=12.06
b=34.07
da=1.5
db=1.6
x = np.matrix([[a, b, da, db]]).T  #和z的定义类似哈

3.3 状态转移矩阵F

       这个矩阵可以简单地理解为前一个状态向量到下一个状态向量的乘法系数。比如小车在平面上匀速运行，那么状态转移规则是:

$$\{\begin{array}{l}{a}_k=a_{k-1}+\Delta a_k\\ b_k=b_{k-1}+\Delta b_k\end{array}$$
这里面的Δa和Δb，就是单位时间内小车在两个坐标轴方向的位移，变相理解为速度，这样状态转移可以表达为：
$$\left(\begin{array}{c}{a}_k\\ b_k\\ \Delta a_k\\ \Delta b_k\end{array}\right)=\left|\begin{array}{cccc}1& 0& 1& 0\\ 0& 1& 0& 1\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right|\left(\begin{array}{c}{a}_{k-1}\\ b_{k-1}\\ \Delta a_{k-1}\\ \Delta b_{k-1}\end{array}\right)$$
上面的这个4*4的矩阵，便是F，可以看出，F的维数为[x_DIM,x_DIM]。比较省心的是，F矩阵一般是个常量。

3.4 观测矩阵H

       观测矩阵表达观测量z和状态量x之间的关系。有些是直接观测两者等效的，比如状态量是室内温度，观测量温度计的输出值，此时观测值和状态值等同，无需变换；也可能是间接观测，比如测量高炉内壁温度，只能通过测量外壁温度然后进行换算。在卡尔曼滤波中，是这样定义观测量z和状态量x之间的计算关系:
$$z_k=H{x}_k+v_k$$
       在实际建模过程中，$u_k$项是无法建模的，所以可以忽略。在本文中，由于是直接观测，这个转移规则就比较简单了：

$$z=\left(\begin{array}{c}{a}_k\\ b_k\end{array}\right)=\left|\begin{array}{cccc}1& 0& 1& 0\\ 0& 1& 0& 1\end{array}\right|\left(\begin{array}{c}{a}_k\\ b_k\\ \Delta a_k\\ \Delta b_k\end{array}\right)=Hx$$
根据$z=Hx$可以得出
$$H=\left|\begin{array}{cccc}1& 0& 1& 0\\ 0& 1& 0& 1\end{array}\right|$$
可以看出，H的维数为[z_DIM,x_DIM]，即2行4列。H矩阵一般也是个常量。

3.5 误差协方差矩阵P

       简单地说，单个变量我们可以用方差来考察它的离散度，对于多个变量它们之间的关联离散度，一般用协方差来表示。P矩阵表示是状态估计的协方差矩阵，它描述了状态估计和真实状态之间的不确定性。
       由此看来，矩阵P的维度由状态量x的维度决定，P的维度就是[x_DIM,x_DIM]的对角矩阵（对角矩阵是啥？不会这么快就忘记了吧，前面有介绍）。这个矩阵可以理解为一个变量，是要i++反复迭代计算的哈！
       P矩阵的初始值应根据系统的具体情况和初始状态估计的精度来确定。如果初始状态估计的精度较高，则P矩阵可以设置为较小的值；反之，则应设置为较大的值。随着不断地迭代计算，P矩阵的初始值对滤波效果的影响在滤波过程中会逐渐减小，因为滤波器会根据观测数据不断更新状态估计和协方差矩阵。因此，即使初始值设置不够准确，滤波器也能在一段时间后收敛到稳定状态。

3.6 过程噪声协方差矩阵Q

       Q矩阵表示系统模型中过程噪声的协方差矩阵，和P一样，既然是与状态有关的协方差，毫不客气地就是[x_DIM,x_DIM]的对角矩阵。
       Q矩阵的初始值应根据系统的动态特性和预期的噪声水平进行估计。如果系统状态变化较快或噪声水平较高，则Q矩阵应设置为较大的值；反之，则应设置为较小的值。简单起见，一般将Q的对角值初始化为较小值（比如0.001)。
       Q矩阵的设置对滤波器的性能有重要影响。如果Q过小，滤波器可能过于依赖模型预测，导致对实际变化的响应不足；如果Q过大，滤波器则可能过于依赖观测数据，导致滤波结果过于嘈杂。因此，在实际应用中，通常需要通过实验或系统辨识等方法来确定Q矩阵的合适值。
       可以理解为Q是一个常量，但是这个常量的数值，需要通过实践反复检测反复调整，才能达到理想的结果。或者理解为手工常量：）

3.7 测量噪声协方差矩阵R

       顾名思义，这个矩阵的维度肯定是由测量z的维度来确定，并且是协方差，那R的维度肯定就是就是[z_DIM,z_DIM]的对角矩阵。R矩阵与测量噪声有关，这个由传感器本身的特性决定。
       在确定R之前，还是需要对测量数据误差做一个初步的分析。R矩阵的初始值通常通过对观测数据进行统计分析来估计，如果观测数据的噪声水平较高，则R矩阵应设置为较大的值；反之，则应设置为较小的值。
       讨厌的是，R与Q矩阵类似，R矩阵的设置也对滤波器的性能有重要影响。如果R过大，滤波器对观测数据的依赖减少，可能导致响应变慢；如果R过小，则可能使滤波结果受到观测噪声的较大影响。因此，在实际应用中，需要通过实验或统计分析等方法来确定R矩阵的合适值。在调试过程中，可以先定住一头，比如先固定一个噪声参数（如先固定R），调整另一个（如Q），观察滤波器的性能变化。开始时可以将R设置得较大，Q设置得较小，然后逐步调整以找到最佳平衡点。
R也可以理解为手工常量。

3.8 卡尔曼增益矩阵K

       这个是鼎鼎有名的系数呵！它也可以理解为一个变量，是要j++的，会反复迭代计算的哈！
       K矩阵可以理解为一个累积系数，它与观测矩阵H转置以后的维度一样，所以在这里K就是[x_DIM,z_DIM]矩阵。
       卡尔曼滤波器中增益可理解为反馈控制中的控制器，而这个控制器中的参数是时刻变化的。如果系统是可观和可控的，则卡尔曼滤波稳定，反映在卡尔曼滤波方程中为方差矩阵P趋于一个常数，进而卡尔曼增益K慢慢收敛到成为一个常数。可以简单理解，K虽然是变量，但是会慢慢从善如流。

3.9 单位矩阵I

       在公式中，I的维度取决x状态的维度，就是[x_DIM,x_DIM]，主对角线上的元素均为1，除此以外全都为0。

3.10 类型总结

       不能老用Python，也要考虑一下c语言大咖们的感受！归纳一下，我们将公式中用到的向量和矩阵定义为数据类型：、

#define z_DIM  2 //测量
#define x_DIM  4 //状态
#define MAX_ELE 4  //本次任务中最大的维数(x/z向量)
typedef struct  {
    int row, col;
    float ele[MAX_ELE][MAX_ELE];
    unsigned char init;
}TMatrix;

void Init_Matrix(TMatrix *pM,int row, int col)
{
    memset(pM,0,sizeof(TMatrix));
    pM->row=row; 
    pM->col=col;
}

TMatrix z;  //测量向量z
TMatrix x;  //状态向量x
TMatrix x_prediction;//x的预测量
TMatrix F; //状态转移矩阵F
TMatrix P; //误差协方差矩阵P
TMatrix P_prediction;//P的预测量
TMatrix Q;//过程噪声协方差矩阵Q
TMatrix K;//卡尔曼增益矩阵K 与观测矩阵H转置以后的维度相同
TMatrix H; //观测矩阵H 
TMatrix HT;//H转置，由于HT经常用到，所以专门将它定义为一个变量
TMatrix I; //单位矩阵I

static void Init_Dim(void)
{
    Init_Matrix(&z,z_DIM，1);           //测量向量z
    Init_Matrix(&x,x_DIM，1);           //状态向量x
    Init_Matrix(&x_prediction,x_DIM，1);//x的预测量
    Init_Matrix(&F,x_DIM,x_DIM);       //状态转移矩阵F
    Init_Matrix(&P,x_DIM,x_DIM);       //误差协方差矩阵P
    Init_Matrix(&P_prediction,x_DIM,x_DIM); //P的预测量
    Init_Matrix(&Q,x_DIM,x_DIM);     //过程噪声协方差矩阵Q
    Init_Matrix(&R,z_DIM,z_DIM);     //测量噪声协方差矩阵R
    Init_Matrix(&K,x_DIM,z_DIM);    //卡尔曼增益矩阵K 与观测矩阵H转置以后的维度相同
    Init_Matrix(&H,z_DIM,x_DIM);    //观测矩阵H 
    Init_Matrix(&HT,x_DIM,z_DIM);   //观测矩阵H 
    Init_Matrix(&I,x_DIM,x_DIM);    //单位矩阵I
}

       定义的TMatrix可能有时候ele有冗余（比如可能是4*2的矩阵），但是这样比alloc等动态分配还是要方便一些，尤其是针对嵌入式应用，尽量用静态的变量，而不用动态分配，这据说也是航天级的经验。
       是不是看代码有亲切感，比看公式来舒服的多？

4 结语

       卡尔曼滤波公式中，有常量有变量。在滤波器的P、Q、R初始化过程中，要根据系统的具体情况和初始状态估计的精度来合理设置这些参数。同时，需要注意调整这些参数对滤波器性能的影响，并通过实验或统计分析等方法来确定它们的合适值，这些是需要有耐心的，是技术活哈。
       下一篇将结合几种常用的场景，看看怎么样的具体应用，总不能光说不练吧。