无人机常用算法——卡尔曼滤波器（十）

sigma

无人机常用算法——卡尔曼滤波器（十） [复制链接]

4 卡尔曼滤波的相关知识

4.1 线性系统

对于线性系统的判断。当描述动态系统的数学方程具有线性属性时，称相应的系统为线性系统。线性系统的基本特征是满足叠加原理，即若表示系统的数学描述为 L，那么对任意两个输入变量 u1和 u2 以及任意两个有限常数c1 和 c2 ，必有

从网上得到如下说明：

状态变量和输出变量对于所有可能的输入变量和初始状态都满足叠加原理的系统。叠加原理是指:如果系统相应于任意两种输入和初始状态(u1,x01)和(u2(t),x02)时的状态和输出分别为(x1(t),y1(t))和(x2(t),y2(t)), 则当输入和初始状态为(C1u1(t)+C2u2(t),C1x01＋C2x02)时，系统的状态和输出必为(C1x1(t)＋C2x2(t),C1y1(t)＋C2y2(t)),其中 x 表示状态,y 表示输入，u 表示输出，C1 和 C2 为任意实数。一个由线性元部件所组成的系统必是线性系统。但是，相反的命题在某些情况下可能不成立。线性系统的状态变量(或输出变量)与输入变量间的因果关系可用一组线性微分方程或差分方程来描述，这种方程称为系统的数学模型。作为叠加性质的直接结果，线性系统的一个重要性质是系统的响应可以分解为两个部分：零输入响应和零状态响应。前者指由非零初始状态所引起的响应；后者则指由输入引起的响应。两者可分别计算。这一性质为线性系统的分析和研究带来很大方便。

4.2 泰勒级数展开

设函数 f(x)在 x=a 处展开，展开公式为：

对于非线性系统的状态方程为：

X(k+1)=f(k,X(k))+W(k) ，其中输暂无输入项。

为了的到一步预测状态 X (k +1| k) ，对上式中的非线性函数在 X (k | k) （已经是一个具体的数值）附近进行泰勒级数展开，取其一阶项。完整的泰勒级数展开为：

其中：n 为状态向量的维数，ei 为第Ⅰ个笛卡尔基本向量，例如四维向量情况下的笛卡尔基本向量有4个：

并且 f(k,X(k))的一阶导数 fX (k)  ：

也是向量 f 的雅可比矩阵，在状态的最近估计X (k | k) 上取值。

类似地可求向量 f 的第 i 个分量的海赛矩阵为：

可见二阶泰勒展开的计算量很大，所以一般取其一阶展开。状态方程的一阶泰勒展开线性化方程为：X(k+1)= f(k,X(k|k))+ f(k)[X(k)-X(k|k)]+W(k)

对一个函数进行泰勒级数一阶展开就是对该函数进行了线性化，如果取其二阶展开则只是对原来非线性函数的另一种形式的近似，仍然是非线性的。