矩阵与状态转移方程
高维高斯函数
均值现在是一个向量，每个维度对应一个元素，方差变为协方差。协方差定义的是高斯函数的分散
当高斯函数倾斜时，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本文参与 腾讯云自媒体同步曝光计划，分享自作者个人站点/博客。
原始发表：2018.10.21 ，如有侵权请联系 cloudcommunity@tencent.com 删除
评论
登录后参与评论
推荐阅读
目录
腾讯云开发者
