矩阵与状态转移方程

高维高斯函数

均值现在是一个向量，每个维度对应一个元素，方差变为协方差。协方差定义的是高斯函数的分散

当高斯函数倾斜时，X和Y的不确定性是相关联的。

卡尔曼滤波器预测

对于卡尔曼滤波器，我们将构建二维估计，一个针对位置，一个针对速度
如果：知道位置但是速度不确定，则高斯分布表示为在正确位置周围的细长分布

卡尔曼滤波器方程式

其中，表示为一个估计值，为了让方程看起来更为简洁：

• 去掉的帽子符号
• 
  最终我们得到：
  
  
  
  
  其中，小写变量表示向量，大写变量表示矩阵

变量定义

—状态向量
—状态转移矩阵
—误差协方差矩阵
—测量噪声协方差矩阵
—计算卡尔曼增益中间矩阵
—卡尔曼增益
—卡尔曼增益
—预测状态与测量状态之差
—测量矢量(激光雷达数据或雷达数据等)
—Identity matrix 单位矩阵

预测步骤方程

预测状态向量与误差协方差矩阵

更新步骤方程

卡尔曼增益


更新状态向量与误差协方差矩阵

矩阵乘法

将矩阵乘法分解成四个步骤：

• get_row(matrix, row_number)
• get_column(matrix, column_number)
• dot_product(vectorA, vectorB)
• matrix_multiply(matrixA, matrixB)

def get_row(matrix, row):
    return matrix[row]
def get_column(matrix, column_number):
    column = []
    for i in range(len(matrix)):
        column.append(matrix[i][column_number])
    return column

def dot_product(vector_one, vector_two):
    sum = 0
    for i in range(len(vector_one)):
        sum = sum + vector_one[i] * vector_two[i]
    return sum
def matrix_multiplication(matrixA, matrixB):
    A = len(matrixA)
    B = len(matrixB[0])
    ### HINT: The len function in Python will be helpful
    m_rows = A
    p_columns = B
    result = []
    row_result = []
    for i in range(m_rows):
        row_vector = get_row(matrixA, i)
        for j in range(p_columns):
            column_vector = get_column(matrixB,j)
            product = dot_product(row_vector, column_vector)
            row_result.append(product)
        result.append(row_result)
        row_result = []
    return result

矩阵转置

def transpose(matrix):
    matrix_transpose = []
    row_result = []
    row = len(matrix)
    column = len(matrix[0])
    for j in range(column):
        for i in range(row):
            row_result.append(matrix[i][j])
        matrix_transpose.append(row_result)
        row_result = []
    return matrix_transpose

利用转置实现矩阵乘法

def dot_product(vector_one, vector_two):
    sum = 0
    for i in range(len(vector_one)):
        sum = sum + vector_one[i] * vector_two[i]
    return sum
def matrix_multiplication(matrixA, matrixB):
    product = []
    row_result = []
    
    ## Take the transpose of matrixB and store the result
    ##       in a new variable
    matrixB_transpose = transpose(matrixB)
    
    ## Use a nested for loop to iterate through the rows
    ## of matrix A and the rows of the tranpose of matrix B
    for i in range(len(matrixA)):
        for j in range(len(matrixB_transpose)):
            dot_result = dot_product(matrixA[i], matrixB_transpose[j])
            row_result.append(dot_result)
    ## TODO: Calculate the dot product between each row of matrix A
    ##         with each row in the transpose of matrix B
        product.append(row_result)
        row_result = []
    return product

生成单位阵

def identity_matrix(n):
    
    identity = []
    row = []
    # Write a nested for loop to iterate over the rows and
    # columns of the identity matrix. Remember that identity
    # matrices are square so they have the same number of rows
    # and columns
    for i in range(n):
        for j in range(n):
            if i == j:
                row.append(1)
            else:
                row.append(0)
        identity.append(row)
        row = []
    # Make sure to assign 1 to the diagonal values and 0 everywhere
    # else
    
    return identity