首页
学习
活动
专区
圈层
工具
发布
社区首页 >专栏 >矩阵与状态转移方程

矩阵与状态转移方程

作者头像
小飞侠xp
发布于 2018-11-08 10:47:23
发布于 2018-11-08 10:47:23
1.5K0
举报
高维高斯函数

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

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

卡尔曼滤波器预测

对于卡尔曼滤波器,我们将构建二维估计,一个针对位置

,一个针对速度

如果:知道位置但是速度不确定,则高斯分布表示为在正确位置周围的细长分布

卡尔曼滤波器方程式

其中,

表示为一个估计值,为了让方程看起来更为简洁:

  • 去掉

的帽子符号

最终我们得到:

其中,小写变量表示向量,大写变量表示矩阵

变量定义

—状态向量

—状态转移矩阵

—误差协方差矩阵

—测量噪声协方差矩阵

—计算卡尔曼增益中间矩阵

—卡尔曼增益

—卡尔曼增益

—预测状态与测量状态之差

—测量矢量(激光雷达数据或雷达数据等)

—Identity matrix 单位矩阵

预测步骤方程

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

更新步骤方程

卡尔曼增益

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

矩阵乘法

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

  • get_row(matrix, row_number)
  • get_column(matrix, column_number)
  • dot_product(vectorA, vectorB)
  • matrix_multiply(matrixA, matrixB)
代码语言:javascript
复制
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
矩阵转置
代码语言:javascript
复制
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

利用转置实现矩阵乘法

代码语言:javascript
复制
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
生成单位阵
代码语言:javascript
复制
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
本文参与 腾讯云自媒体同步曝光计划,分享自作者个人站点/博客。
原始发表:2018.10.21 ,如有侵权请联系 cloudcommunity@tencent.com 删除
目录
  • 高维高斯函数
  • 卡尔曼滤波器预测
  • 卡尔曼滤波器方程式
  • 变量定义
  • 预测步骤方程
  • 更新步骤方程
  • 矩阵乘法
  • 矩阵转置
  • 生成单位阵
问题归档专栏文章快讯文章归档关键词归档开发者手册归档开发者手册 Section 归档