測量模型(update模型)

預測模型中極有可能包含了規(guī)劃器所產生的誤差，因此，我們可以使用更多的測量模型來糾正預測值，以獲得更精確的接觸概率估計。 標準卡爾曼濾波修正方程如下：

1、地形高度模型

同樣，取不同參數觀察其概率分布情況，橫坐標為地形高度，縱坐標為發(fā)生接觸的概率：

高度模型的代碼：

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、反作用力模型

取不同參數觀察其概率分布圖像，橫坐標為力的大小，縱坐標為發(fā)生接觸的概率：

模型代碼如下：

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

該模型提供第二項校正測量結果：