离散卡尔曼滤波实现

离散卡尔曼滤波基本理论

卡尔曼预报器、平滑器可以参考之前的博客：(2条消息) 卡尔曼滤波器_KPer_Yang的博客-优快云博客

下面贴上一张图¹，很直观：分成时间更新和测量更新两步，其中的$K$和$P$有可能随着时间推移变成常数，但是多数情况是变化的，如果是常数，可以离线计算在线使用，可以减少很多在线计算量。$K$代表着观测值和状态空间计算值的不同比重，得到一个估计值作为输出。初始化观测噪声协方差矩阵$R$可以使用离线数据。初始化过程噪声矩阵$Q$比较困难。可以使用另一个卡尔曼滤波器离线进行估计，再实际使用。

陀螺仪的卡尔曼滤波参数初始化可以参考这种（其中q和r参数尤为重要，一般得通过实验测试得到）：²

1. init_x：待测量的初始值，如有中值一般设成中值
2. init_p：后验状态估计值误差的方差的初始值
3. q：预测（过程）噪声方差
4. r：测量（观测）噪声方差。以陀螺仪为例，测试方法是：保持陀螺仪不动，统计一段时间内的陀螺仪输出数据。数据会近似正态分布，按3σ原则，取正态分布的(3σ)^2作为r的初始化值。