卡尔曼滤波-EKF

EKF

EKF称为扩展卡尔曼滤波，他和标准卡尔曼滤波有何区别呢？他的出现主要是为了解决在非线性系统下的最优估计问题，前面我们提到标准卡尔曼滤波只适用于线性系统，对于非线性系统我们可以用EKF来进行最优估计。

GitHub demo

AHRS_EKF_Matlab

线性与非线性

https://zhuanlan.zhihu.com/p/84365683
线性（Linear）系统的一阶导数为常数，线性系统满足比例性和叠加性。
可以用简单的一句话来描述线性系统：若该系统的的微分方程满足线性性，则该系统为
线性系统，但是对于线性系统来说，一定要先区分系统的输入与输出，再来分析线性系统。
例如函数：f(t)=sin(t)r(t)，此时 r(t) 才是输入，t 不是直接的输入变量，所以即使 sin(t) 不
是线性的，但该系统仍是线性的。

雅克比矩阵（Jacobian Matrix）

非线性系统线性化我们需要用到雅克比矩阵，实际上也就是求一阶偏导数，对于非线性进行线性化有很多种方法，比如：泰勒展开，最小二乘法，雅可比矩阵，傅里叶展开。对雅克比矩阵感兴趣的可以到雅可比矩阵是什么东西-3Blue1Brown了解更多，在工程上我们一般直接借助MATLAB或者PYTHON直接计算jabobian matrix即可。

高级IMU

前一章我们使用标准卡尔曼滤波实现了姿态估计，但是在实际应用中，我们看到飞控中用的是EKF来进行姿态估计，这是为什么？回顾一下我们之前的系统状态量，是3轴欧拉角，对吧？在实际应用中，由于万向锁的存在，我们不会使用欧拉角来表示姿态，在全姿态的模型中，我们使用四元数来表示姿态，关于欧拉角，四元数，旋转矩阵这些我在系列文章的第一章就进行了全部描述，感兴趣的朋友可以回去看一看。

四元数微分方程

现在我们的系统状态量变成了四元数，也就是我们的系统模型发生了改变，第一步，我们要得到我们的系统模型动力方程，也就是状态预测方程。

状态预测

协方差矩阵预测

得到观测矩阵

计算残差

计算卡尔曼增益

状态更新

矩阵的定义和初始化

总结