轨迹平滑python

sg滤波：

# =============================================================================
# Savitzky-Golay滤波拟合法
# =============================================================================
def optimizePoints(inPoint):
    """
        5点1阶段轨迹平滑算法
        input
            inPoint list点序列 [{'lng':0.0, 'lat':0.0},...]
        output
            inPoint 滤波后的list点序列 [{'lng':0.0, 'lat':0.0},...]
    """
    size = len(inPoint)
    
    outinPoint = copy.deepcopy(inPoint)
    
    if (size < 5):
        return inPoint
    else:
        # Latitude
        outinPoint[0]['lat'] = (3.0 * inPoint[0]['lat'] + 2.0 * inPoint[1]['lat'] + inPoint[2]['lat'] - inPoint[4]['lat']) / 5.0
        outinPoint[1]['lat'] = (4.0 * inPoint[0]['lat'] + 3.0 * inPoint[1]['lat'] + 2 * inPoint[2]['lat'] + inPoint[3]['lat']) / 10.0
        for i in range(2, size-3):
            outinPoint[i]['lat'] = (inPoint[i-2]['lat'] + inPoint[i-1]['lat'] + inPoint[i]['lat'] + inPoint[i+1]['lat'] + inPoint[i+2]['lat']) / 5.0
        outinPoint[size-2]['lat'] = (4.0 * inPoint[size-1]['lat'] + 3.0 * inPoint[size-2]['lat'] + 2 * inPoint[size-3]['lat'] + inPoint[size-4]['lat']) / 10.0
        outinPoint[size-1]['lat'] = (3.0 * inPoint[size-1]['lat'] + 2.0 * inPoint[size-2]['lat'] + inPoint[size-3]['lat'] - inPoint[size-5]['lat']) / 5.0
        
        # Longitude
        outinPoint[0]['lng'] = (3.0 * inPoint[0]['lng'] + 2.0 * inPoint[1]['lng'] + inPoint[2]['lng'] - inPoint[4]['lng']) / 5.0
        outinPoint[1]['lng'] = (4.0 * inPoint[0]['lng'] + 3.0 * inPoint[1]['lng'] + 2 * inPoint[2]['lng'] + inPoint[3]['lng']) / 10.0
        for i in range(2, size-3):
            outinPoint[i]['lng'] = (inPoint[i-2]['lng'] + inPoint[i-1]['lng'] + inPoint[i]['lng'] + inPoint[i+1]['lng'] + inPoint[i+2]['lng']) / 5.0
        outinPoint[size - 2]['lng'] = (4.0 * inPoint[size-1]['lng'] + 3.0 * inPoint[size-2]['lng'] + 2 * inPoint[size-3]['lng'] + inPoint[size-4]['lng']) / 10.0
        outinPoint[size - 1]['lng'] = (3.0 * inPoint[size-1]['lng'] + 2.0 * inPoint[size-2]['lng'] + inPoint[size-3]['lng'] - inPoint[size-5]['lng']) / 5.0
    
    return outinPoint