英文:
Kalman Filtering in Python
问题
我一直在尝试设计卡尔曼滤波器已经几周了,但我相当确定我犯了一个严重错误,因为我的结果很糟糕。我的常识告诉我,这可能是因为我使用已存在的矩阵作为预测状态,而不是使用过渡矩阵,但如果确实是这个问题,我不确定如何解决它。顺便说一下,这是我第一次使用卡尔曼滤波,所以我可能错过了一些基本的东西。
以下是我的代码的主要部分:
import numpy as np
#观测数量
nn=81036
#数据点数量
ns=6
#导入数据
ps=np.genfromtxt('.......csv', delimiter=',')
ms=np.genfromtxt('.......csv', delimiter=',')
##带协方差的卡尔曼滤波
#初始化数据(使用列的均值进行惰性初始化)
xi=np.mean(ms,axis=0)
for i in np.arange(nn):
#误差
d=ms[i,:]-xi
d2=ps[i,:]-xi
#协方差矩阵
P=np.zeros((ns,ns))
R=np.zeros((ns,ns))
for j in np.arange(ns):
for s in np.arange(ns):
P[j,s]=d[j]*d展开收缩
R[j,s]=d2[j]*d2展开收缩
#增益
k=P*(P+R)**-1
#更新估计
xi=xi+np.matmul(k,d2)
#不确定性/误差
I=np.identity(ns)
mlt=np.matmul((I-k),P)
mlt=np.matmul(mlt,((I-k).T))
mlt2=np.matmul(k,R)
mlt2=np.matmul(mlt2,k.T)
Er=mlt+mlt2
当我运行这段代码时,我的滤波状态xi的值飙升,所以我相当确定这不是正确的代码。我已经尝试了多种方式来修复它(例如,我尝试按我习惯的标准方式计算协方差矩阵 - D'D/n -,我尝试移除我的预测状态矩阵,而是向我的测量状态添加随机噪声...),但似乎没有什么效果。我还尝试了一些可用的卡尔曼滤波库(以及Matlab和R中的库),但它们要么只在1D中工作,要么需要我指定过渡矩阵等变量,而我没有这些信息。我已经不知所措了,所以我会感激任何帮助。
英文:
I've been trying to work on designing a Kalman Filter for a few weeks now, but I'm pretty sure I'm making a major error because my results are terrible. My common sense tells me it's because I'm using an already-existing matrix as my predicted state instead of using a transition matrix, but I'm not sure how to solve that if it indeed is the issue. By the way, this is my first time using Kalman Filtering, so I may be missing basic stuff.
Here is a detailed explanation:
I have 2 datasets of 81036 observations each, with each observation including 6 datapoints (i.e., I end up with 2 matrices of shape 81036 x 6). The first dataset is the measured state and the other one is the predicted state. I want to end up with a Python code that filters the data using both states, and I need the final covariance and error estimates. Here's the main part of my code:
import numpy as np
#nb of observations
nn=81036
#nb of datapoints
ns=6
#import
ps=np.genfromtxt('.......csv', delimiter=',')
ms=np.genfromtxt('.......csv', delimiter=',')
##kalman filtering with covariance
#initialize data (lazy initialization using means of columns)
xi=np.mean(ms,axis=0)
for i in np.arange(nn):
#errors
d=ms[i,:]-xi
d2=ps[i,:]-xi
#covariance matrices
P=np.zeros((ns,ns))
R=np.zeros((ns,ns))
for j in np.arange(ns):
for s in np.arange(ns):
P[j,s]=d[j]*d展开收缩
R[j,s]=d2[j]*d2展开收缩
#Gain
k=P*(P+R)**-1
#Update estimate
xi=xi+np.matmul(k,d2)
#Uncertainty/error
I=np.identity(ns)
mlt=np.matmul((I-k),P)
mlt=np.matmul(mlt,((I-k).T))
mlt2=np.matmul(k,R)
mlt2=np.matmul(mlt2,k.T)
Er=mlt+mlt2
When I run this code, I end up with my filtered state xi going through the roof, so I'm pretty sure this is not the correct code. I've tried to fix it in several ways (e.g., I tried to calculate the covariance matrix in the standard way I'm used to - D'D/n -, I tried to remove my predicted state matrix and simply add random noise to my measured state instead...), but nothing seems to work. I also tried some available libraries for Kalman Filtering (as well as libraries in Matlab and R), but they either work in 1D only or need me to specify variables like the transitional matrix, which I don't have. I'm at the end of my wits here, so I'd appreciate any help.
答案1
得分: 0
我已找到解决这个问题的方法。非常感谢Kani的评论,因为它指导了我正确的方向。
原来问题出在计算k的地方。虽然方程是正确的,但由于某些情况下R和P的值非常小,逆函数无法正常工作。为了解决这个问题,我改用了伪逆矩阵,因此计算k的代码如下:
k = P @ np.linalg.pinv(P + R)
请注意,在其他情况下,这可能不如逆矩阵精确,但在这里它起到了作用。
英文:
I've found the solution to this issue. Huge props to Kani for their comment, as it pointed me in the right direction.
It turns out that the issue is simply in the calculation of k. Although the equation is correct, the inverse function was not working properly because of the very small values in some instances of R and P. To solve this, I used the pseudoinverse instead, so the line for calculating k became as follows:
k = P @ np.linalg.pinv(P + R)
Note that this might not be as accurate as the inverse in other cases, but it does the trick here.
通过集体智慧和协作来改善编程学习和解决问题的方式。致力于成为全球开发者共同参与的知识库,让每个人都能够通过互相帮助和分享经验来进步。
评论