Simultaneous localization and Ma

Graph SLAM

Graph SLAM 是众多SLAM方法中的一种

约束：
对于6个姿势：

• 一初始位置约束
• 5个附加相对运动约束
• 8个地标位置相对测量约束
  所有这些加起来总共14个约束

考虑上图，有4个姿势(包括初始位置x0)和一个地标。我们可以用同样的数学方法，对给定的图像有5个总约束。
你可能注意到，不是所有的约束都会提供有用的信息。例如：图中X2没有和地标之间进行测量。

将两次约束相加：

添加地标：

将没有联系的矩阵置为0：

为了实现Graph SLAM ,引入了，该矩阵为方正，并标有所有机器人姿势和所有地标。当在两个姿势移动距离,将这两个位置关联起来，可以将其表示为这些矩阵中的数值关系。下图为的一个矩阵表示和的一个向量表示：

为了确定姿势和地标位置

• 初始位置 -3
• 移动 5
• 移动 3

约束更新

当机器人移动一定量,需要更新约束矩阵，如下所示：

• 在 xt和xt+1交叉点的索引处将 [[1, -1], [-1, 1]]添加到omega中
• 在xt和xt+1行中将 -dx 和dx添加到xi中

import numpy as np


def mu_from_positions(initial_pos, move1, move2):
    
    ## TODO: construct constraint matrices
    ## and add each position/motion constraint to them
    
    # Your code here
    omega = np.zeros((3,3))
    xi = np.zeros((3,1))
    
    omega[0][0] = 1
    # account for the first motion, dx = move1
    xi[0] = initial_pos
    
    omega += [[1., -1., 0.],
              [-1., 1., 0.],
              [0., 0., 0.]]
    
    xi += [[-move1],
           [move1],
           [0.0]]
    
    # account for the second motion
    omega += [[0., 0., 0.],
              [0., 1., -1.],
              [0., -1., 1.]]
    
    xi += [[0.],
           [-move2],
           [move2]]
    # display final omega and xi
    
    
    print('Omega: \n', omega)
    print('\n')
    print('Xi: \n', xi)
    print('\n')
    
    ## TODO: calculate mu as the inverse of omega * xi
    ## recommended that you use: np.linalg.inv(np.matrix(omega)) to calculate the inverse
    omega_inv = np.linalg.inv(np.matrix(omega))
    mu = omega_inv*xi
    return mu

# call function and print out `mu`
mu = mu_from_positions(-3, 5, 3)
print('Mu: \n', mu)

测量信任强度

传感器测量

当感测到姿势和地标之间的距离dl时，更新约束矩阵，如下所示：

• 在 xt和l交叉点的索引处将 [[1, -1], [-1, 1]]添加到omega中
• 在xt和l行中将 -dl 和dl添加到xi中
  三个新的dl值作为函数Z0, Z1, Z2的新输入，如下所示。

运动

当机器人移动一定量dx时，更新约束矩阵时，如下所示：

• 在xt 和 xt+1 交叉点的索引处将[[1, -1], [-1, 1]]添加到omega中
• 在xt 和 xt+1 的行中将-dx 和 dx 添加到xi中

更改以上代码，使最后一次传感器测量(Z2)的置信度非常高，将该测量的强度乘以5倍

import numpy as np
def mu_from_positions(initial_pos, move1, move2, Z0, Z1, Z2):
    
    ## construct constraint matrices
    ## and add each position/motion constraint to them
    
    # initialize constraint matrices with 0's
    # Now these are 4x4 because of 3 poses and a landmark
    omega = np.zeros((4,4))
    xi = np.zeros((4,1))
    
    # add initial pose constraint
    omega[0][0] = 1
    xi[0] = initial_pos
    
    # account for the first motion, dx = move1
    omega += [[1., -1., 0., 0.],
              [-1., 1., 0., 0.],
              [0., 0., 0., 0.],
              [0., 0., 0., 0.]]
    xi += [[-move1],
           [move1],
           [0.],
           [0.]]
    
    # account for the second motion
    omega += [[0., 0., 0., 0.],
              [0., 1., -1., 0.],
              [0., -1., 1., 0.],
              [0., 0., 0., 0.]]
    xi += [[0.],
           [-move2],
           [move2],
           [0.]]
    ## Include three new sensor measurements for the landmark, L
    # incorporate first sense
    omega += [[1., 0., 0., -1.],
              [0., 0., 0., 0.],
              [0., 0., 0., 0.], 
              [-1., 0., 0., 1.]]
    xi += [[-Z0],
           [0.0],
           [0.0],
           [Z0]]

    # incorporate second sense
    omega += [[0., 0., 0., 0.],
              [0., 1., 0., -1.],
              [0., 0., 0., 0.], 
              [0., -1., 0., 1.]]
    xi += [[0.],
           [-Z1],
           [0.],
           [Z1]]
    
    ## This third sense is now *very confident* and
    ## we multiply everything by a strength factor of 5 instead of 1
    # incorporate third sense
    omega += [[0., 0., 0., 0.],
              [0., 0., 0., 0.],
              [0., 0., 5., -5.], 
              [0., 0., -5., 5.]]
    xi += [[0.],
           [0.],
           [-Z2],
           [Z2]]
    
    # display final omega and xi
    print('Omega: \n', omega)
    print('\n')
    print('Xi: \n', xi)
    print('\n')
    
    ## calculate mu as the inverse of omega * xi
    ## recommended that you use: np.linalg.inv(np.matrix(omega)) to calculate the inverse
    omega_inv = np.linalg.inv(np.matrix(omega))
    mu = omega_inv*xi
    return mu

# call function and print out `mu`
mu = mu_from_positions(-3, 5, 3, 10, 5, 1)
print('Mu: \n', mu)