Simulink卡尔曼滤波系统建模实例——高度解算融合

前言

这个项目主要是利用加速度计和气压计作为输入，经过卡尔曼滤波融合后输出高度、速度、加速度，这三个输出量对于产品的某执行机构来说需要满足符合无人机飞行特征，而这个项目也是基于原算法在Simulink上面的复现以及优化。

void update_sys() {
   
   
	X_hat = F * X;
	P_hat = F * P * F.transpose() + Q;
	P_inv = H * P_hat * H.transpose() + R;
	K	  = P_hat * H.transpose() * inv(P_inv);
	X	  = X_hat + K * (Y - H * X_hat);
	P	  = (I - K * H) * P_hat;
}

算法模型

算法原理

KF实际意义上是最优化递归数字处理算法（Optimal Recursive Data Processing Algorithm）。在变化的数据中去除噪声对系统未来输出做出预测的算法，是基于概率统计原理的预测算法，算法输入的量是可测的（例如我的实例中的输入就用到了加速度计和气压计测量出来的数据），并且还知道这个测量值的精度大概在多少，有了这个测量值我们就根据测量值来估计这个系统的真实输出，并同时给出我新估计的这个值的精度大概在什么范围内，这就是卡尔曼滤波做的工作，但这个工作是不断进行的，对系统不断测量，然后不断估计，这样持续一段时间之后就能估计出系统一个非常准确的输出值。这里要明确的一点是，测量值可能非常不准确，估计值也非常不准确，这符合工程中的很多工作状况，但仅仅根据这两个不准确的值最后就可以估计出一个相对准确的系统输出值，这也就是卡尔曼滤波的作用。

算法五个重要公式

预测过程：

校正过程：

（这里对公式推导就不做太多的赘述了，因为这篇讲的是Simulink卡尔曼滤波系统建模。）

Simulink模块化建模