卡尔曼滤波器上的状态矩阵Xk与生态大小不匹配

Question

当我试图更新状态矩阵的估计时，我遇到了问题。 因为大小不匹配。

我正在阅读这个优秀的 文件 来了解卡尔曼滤波器背后的德理论。

总的来说矩阵的大小是。

x = 状态向量 => mx*1

z = 输出向量 => mz*1

F = 状态转换矩阵 => mx*nx

P = 估计不确定性矩阵 => mx*nx

Q = 过程噪声无确证矩阵 => mx*nx

R = 测量不确定度矩阵 => mz*nz。

H = 观测矩阵 => mz*nx。

K = Kalman Gain => mx*nz。

在我的实现中，我使用过滤器来跟踪一个对象。我的状态向量是这样的。

x=[px,vx,ax,py,vy,ay];

输出向量

z=[px,py]

状态转换矩阵

F = [
    [ 1, 1, 0.5, 0, 0, 0],
    [ 0, 1, 1, 0, 0, 0],
    [ 0, 0, 1, 0, 0, 0],
    [ 0, 0, 0, 1, 1, 0.5],
    [ 0, 0, 0, 0, 1, 1],
    [ 0, 0, 0, 0, 0, 1]]

方差-方差矩阵

P = [
    [ 3, 2, 1, 0, 0, 0],
    [ 2, 3, 2, 0, 0, 0],
    [ 1, 2, 3, 0, 0, 0],
    [ 0, 0, 0, 3, 2, 1],
    [ 0, 0, 0, 2, 3, 2],
    [ 0, 0, 0, 1, 2, 3]]

过程噪音

Q = [
    [ 0.1, 0.1, 0.1, 0, 0, 0],
    [ 0.1, 0.1, 0.1, 0, 0, 0],
    [ 0.1, 0.1, 0.1, 0, 0, 0],
    [ 0, 0, 0, 0.1, 0.1, 0.1],
    [ 0, 0, 0, 0.1, 0.1, 0.1],
    [ 0, 0, 0, 0.1, 0.1, 0.1]]

测量噪音

R = [
    [3, 0],
    [0, 2]]

观察矩阵

H = [
    [1, 0, 0, 0, 0, 0],
    [0, 0, 0, 1, 0, 0]]

我这样做是因为我只想要结果的位置。(x,y)

运行的急性...

1. 时间更新

1. Extrapolate the state
Xk^=F*Xk_1

这里没有问题，没有控制输入，因为我只是在读取数值，结果就是 mx*1

2. Extrapolate the uncertainty
Pk=F*Pk_1*FT + Q

这里没有问题，尺寸是正确的，结果就像理论所说的那样。mx*nx

2. 计量更新 1. 计算卡尔曼增益

K=Pk*HT*(H*Pk*HT+ R)^-1

这里没有问题，结果和理论一样。mx*nz.

2.Update the estimate uncertainty
Pk'= Pk- K*H*Pk

这里没有问题，结果和预期的一样。mx*nx.

3.Update the estimate (state), with measurement.
Xk=Xk^ + K*(z-H*Xk^)

最后就是这个问题，我现在做的是先计算 K*(z-H*Xk^)......它的结果是一个矩阵与 mz*nz换句话说，（2x2）大小，但是 Xk^ 矢量的大小为 mx*1. 所以，当我尝试添加这两个矩阵，它的结果是一个错误。

我怎么能解决这个问题？我不知道，我不知道我错过了什么。

我使用nodejs来运行这个。但我有一步一步在excel中的spread sheet。

Answer 1

如果我们把

Xk^ + K*(z-H*Xk^)

变成步骤。

H*Xk^ is mz*nx by nx*1 so is mz*1
z - H*Xk^ is mz*1 - mz*1 so is mz*1
K*(z-H*Xk^) is mx*nz by mz*1 so is mx*1

(另外，我发现你使用的mx和nx的用法很混乱。) 这些数字一定是相等的（因为P是正方形）。

Answer 2

(z - H*Xk^)也叫创新，它的尺寸应该是nz x 1。我猜想这就是问题所在。

你可以定义

z^=H*Xk^，其尺寸也应是nz×1

我的猜测是你在z变量中有多个堆叠测量，也就是说，z不是nz*1 ，它实际上是nz x Nz，其中nz是测量的维度，Nz是测量的数量。在这种情况下，你或者可以在将z^矩阵分离成nz x 1向量后，执行连续的单次测量更新，但这意味着你必须能够为堆叠测量向量中的每一次测量计算一个预测值。