Kalmam Filter原理和公式推导 |
您所在的位置:网站首页 › z值三个模型公式 › Kalmam Filter原理和公式推导 |
经典的卡尔曼黄金五条公式 参考链接 卡尔曼滤波器的主要作用卡尔曼滤波器主要是利用了预测和更新两个过程来对系统状态进行最优估计。可以用在多传感器融合上,例如imu,odom,GPS信号的融合位姿估计。也就是利用对其他量的观测去估计无法直接测量的量。 卡尔曼滤波基本模型 \[x_k = A * x_{k-1} + B* u_k + w_{k-1} \\ z_k = H *x_k + v_k\]$x_k $ 是系统状态矩阵,$z_k$ 是系统观测量,$A$ 是状态转移矩阵,$H$ 是状态观测矩阵,$w_{k-1}$ 是过程噪声,$v_k$ 是测量噪声。 其协方差法分别为$Q$ 和 $R$。 $x_k$ : 状态的真实值 \(\tilde{x}_k^-\) : 状态的预测值,也称先验状态估计 $\tilde{x}_k$:状态的最优估计值 ,也称后验状态估计 黄金一条(状态预测方程) \[\tilde{x}_k^- = A * \tilde{x}_{k-1} + B * u_k\]黄金二条(状态更新方程) \[\tilde{x}_k = \tilde{x}_k^- + K(z_k - H * \tilde{x}_k^-)\]黄金三条(卡尔曼增益矩阵) \[K = P_k^-H^T(HP_k^-H^T + R)^{-1}\]黄金四条(估计误差方差阵) \[P_k = (I - KH) * P_k^-\]黄金五条(预测协方差阵) \[P_{k+1}^- = AP_kA^T + Q\] 总结卡尔曼滤波的主要思想就在于预测和更新,初始的协方差阵可以设为单位阵,之后不断地更新预测协方差阵和误差协方差阵,之后计算出K,然后根据K来更新最优估计。 ps:如果卡尔曼滤波没有预测方程会怎么样? 回答:如果没有 \(\tilde{x}_k^- = A * \tilde{x}_{k-1} + B * u_k\) 的话,只有 \(\tilde{x}_k^- = \tilde{x}_{k-1}\) 那么模型的估计误差方差阵会越来越小,导致K越来越接近0,最后机器人会认为自身不动。 |
CopyRight 2018-2019 办公设备维修网 版权所有 豫ICP备15022753号-3 |