0

我有一个卡尔曼滤波器的实现，我正在做矩阵运算。在某些时候，我应该减去两个 1x1 矩阵。我有一个错误，我不知道它来自哪里。在 python 中进行矩阵运算的最佳方法是什么？

import numpy as np
import pylab as pl
import scipy as Sci
import scipy.linalg as linalg

class GetPos(object):
    def __init__(self):
        self.Posp = 0
        self.Velp = 80
        self.z = np.matrix(0)
    def __repr__(self):
        return "from GetPos.__repr__ z=%s" % (self.z)
    def __call__(self):
        self.dt = 0.1
        self.w = 0 + 10*np.random.random()
        self.v = 0 + 10*np.random.random()            
        self.z = self.Posp + self.Velp*self.dt + self.v
        self.Posp = self.z - self.v
        self.Velp = 80 + self.w
        print 'from GetPos.__call__ z = %s' % self.z
        return self.z

class DvKalman(object):
    def __init__(self):
        self.dt = .1
        self.A = np.matrix([[1., self.dt],[0,1]])
        self.H = np.matrix([1., 0])
        self.Q = np.matrix([[1,0.],[0,3]])
        self.R = np.matrix(10)
        self.x = np.matrix([0,20]).T
        self.P = np.matrix(5*np.eye(2))
        #print 'P matrix \n%s' % self.P
        self.firstRun = 0
    def __call__(self, z):
        self.z = z
        print 'from DvKalman.__call__ slef.z = %s and z = %s' % (self.z,z)
        self.xp = self.A * self.x
        self.Pp = self.A*self.P*self.A.T  + self.Q
        self.K = self.Pp * self.H.T * linalg.inv(np.absolute(self.H*self.Pp*self.H.T + self.R));
        print 'from DvKalman.__call__  z=%s, \npreviouse x=\n%s \nH = \n%s \nand P=\n%s \nand xp=\n%s,\n Pp = \n%s,\n K=\n%s' % (self.z,self.x,self.H, self.P,self.xp,self.Pp,self.K)
        newM1 = self.H*self.xp    
        print 'This is self.H*self.xp %s and this is self.z = %s' % (newM1, self.z)
        newM2 = self.z - self.H*self.xp
        print 'This should give simple substruction %s' % newM2                 
        self.x = self.xp + self.K*(self.z - self.H*self.xp)
        self.P = self.Pp - self.K*self.H*self.Pp
        print 'new values x=%s and P=%s' % (self.x,self.P)
        return (self.x)
def TestDvKalman():
    Nsamples = np.arange(0,10,.1)

    kal = DvKalman()
    #print type(kal)
    Xsaved = []
    Zsaved = []

    for i in range(len(Nsamples)):
        z = GetPos()
        print z
        print 'from TestDvKalman zpos = %s' % z
        Zsaved.append(z)
        [position, velocity] = kal(z)
        print position, velocity
        Xsaved.append([position, velocity])
    print Zsaved
    print Xsaved
#    f1 = pl.subplot(121)
#    f1 = pl.plot(Xsaved, 'x-',label = 'Xsaved')
#    f1 = pl.legend()
#    
#    f2 = pl.subplot(122)
#    f2 = pl.title('Kalman Velocity')
#    f2 = pl.plot(Zsaved, 'o-', color = 'brown',label = 'Zsaved')
#    f2 = pl.legend()
#    
#    pl.show()

if __name__ == '__main__':  
    TestDvKalman()

我添加了几print行代码来跟踪和调试代码，并添加newM了代码中不存在的新变量。矩阵打印正确This is self.H*self.xp [[ 2.]] and this is self.z = from GetPos.__repr__ z=[[0]]两个矩阵都是 1x1 但我仍然收到错误，不知道为什么。错误是：

    newM2 = self.z - self.H*self.xp
TypeError: unsupported operand type(s) for -: 'GetPos' and 'matrix'

我怀疑我在某处弄乱了类型，但不知道在哪里以及如何更正它。你能指出错误在哪里以及如何构建这样的代码以避免将来出现类似的错误吗？

4

3 回答 3

2

您正在将 GetPos 实例传递给 DvKalman__call__方法。所以你试图减去一个 GetPos 实例和一个矩阵。不是矩阵和矩阵。

于 2012-10-23T12:38:07.117 回答
2

中TestDvKalman，这条线

    z = GetPos()

设置z为 的一个实例GetPos。kal您在此行中使用它作为参数：

    [position, velocity] = kal(z)

所以给了__call__方法的DvKalman一个实例GetPos，你把它保存为self.z。这导致了这一行的错误：

    newM2 = self.z - self.H*self.xp
于 2012-10-23T12:40:36.927 回答
1

替换newM2 = self.z - self.H*self.xp为newM2 = self.z() - self.H*self.xp。

该程序应该可以使用（但我无法确认它是否确实需要你想要的）

于 2012-10-23T12:52:26.757 回答