四足机器人测量模型介绍

测量模型(update模型)

预测模型中极有可能包含了规划器所产生的误差，因此，我们可以使用更多的测量模型来纠正预测值，以获得更精确的接触概率估计。 标准卡尔曼滤波修正方程如下：

1、地形高度模型

同样，取不同参数观察其概率分布情况，横坐标为地形高度，纵坐标为发生接触的概率：

高度模型的代码：

def ground_height(pz, params):
    """
    The probability of contact given foot heigh


    :param pz: ground height
    :param params: [mu_z, sigma_z]
    :return: The probability of contact
    """
    mu_z, sigma_z = params
    prob_ground_height = 0.5 * (1 + math.erf((mu_z-pz) / (sigma_z*np.sqrt(2))))


    return prob_ground_height

该模型提供的校正测量及其协方差矩阵如下：

2、反作用力模型

取不同参数观察其概率分布图像，横坐标为力的大小，纵坐标为发生接触的概率：

模型代码如下：

def contact_force(f, params):
    """
    the probability of contact given the estimated foot force


    :param f: contact force
    :param params: [mu_z, sigma_z]
    :return: The probability of contact
    """
    mu_f, sigma_f = params
    prob_force = 0.5 * (1 + math.erf((f-mu_f) / (sigma_f*np.sqrt(2))))


    return prob_force

该模型提供第二项校正测量结果：