二维kalman程序是错的？

[demolpc]

    /* p(n|n-1)=A^2*p(n-1|n-1)+q */
    state->p[0][0] = state->A[0][0] * state->p[0][0] + state->A[0][1] * state->p[1][0] + state->q[0];

还有q应该是2乘2的矩阵，这里为什么是二维向量？

[TabDynasty]

确实，他少乘了一个A的转置，但是对于你的第二个问题，一般来说Q和R矩阵都是对角阵，副对角线上的两个值都为零，因为两个误差值一般是独立的，所以他用二维向量来表示是可以的