矩阵与状态转移方程

作者: 徐凯_xp | 来源:发表于2018-10-21 14:20 被阅读0次

矩阵与状态转移方程
【Intro2SDC】卡尔曼方程参考
动态规划
线性代数——2. 矩阵及其运算
2022-02-19 动态规划专题
动态规划
卡尔曼滤波(3)
矩阵
动态规划笔记（二）
07-15：动态规划review3

高维高斯函数

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

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

卡尔曼滤波器预测

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

卡尔曼滤波器方程式

$\hat x_{k|k-1} = F_k\hat x_{k-1|k-1} + B_ku_k$
其中， $\hat x$ 表示为一个估计值，为了让方程看起来更为简洁：

去掉 $x$ 的帽子符号
$\hat x_{k|k-1} \to x'$
最终我们得到：
$x' = Fx +Bu$

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

变量定义

$\hat x$ —状态向量
$F$ —状态转移矩阵
$P$ —误差协方差矩阵
$Q$ —测量噪声协方差矩阵
$R$ —计算卡尔曼增益中间矩阵
$S$ —卡尔曼增益
$K$ —卡尔曼增益
$\widetilde y$ —预测状态与测量状态之差
$z$ —测量矢量(激光雷达数据或雷达数据等)
$I$ —Identity matrix 单位矩阵

预测步骤方程

预测状态向量与误差协方差矩阵
$\hat x_{k|k-1} = F_k\hat x_{k-1|k-1}$
$P_{k|k-1} = F_k P_{k-1|k-1}F_k^T+Q_k$

更新步骤方程

卡尔曼增益
$S_k = H_kP_{k|k-1}H_k^T+R_k$
$K_k = P_{K|k-1}H_k^TS_k^{-1}$
更新状态向量与误差协方差矩阵
$\widetilde y_k = z_k - H_k\hat x_{k|k-1}$
$\hat x_{k|k} = \hat x_{k|k-1} + K_k\widetilde y_k$
$P_{k|k} = (I-K_kH_k)P_{k|k-1}$

矩阵乘法

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

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