Exemplo n.º 1
0
 def update(self,dt,ymeas):
     if self.initFlag:
         MUK = np.zeros(self.XK.shape)
         XKI = np.zeros(self.XK.shape)
         for k in range(self.Ns):
             # compute the expectation of the current particle when propagated to the next state
             # this is probably just the propagateFunction with no process noise applied, but we make no assumptions about this in this class
             MUK[:,k] = self.meanPropagateFunction(self.XK[:,k],dt)
             # also?? propagate the particles through dt?? This step is unclear, but the propagated particles are used in the resample algorithm
             # draw process noise according to the process noise model
             vki = self.processNoiseSample(self.XK[:,k])
             XKI[:,k] = self.propagateParticle(self.XK[:,k],dt,vki)
             # update the current particle's weight based on the measurement PDF
             self.WI[k] = self.WI[k]*self.measurementNoisePdf(ymeas,MUK[:,k])
         # normalize the weights
         weightFactor = 1.0/np.sum(self.WI)
         for k in range(self.Ns):
             self.WI[k] = self.WI[k]*weightFactor
         # use the standard resample function, which depends on the propagated particles?
         (gar1,gar2,ipl) = pu.resample(XKI,self.WI)
         # copies of the prior state and weights
         XKc = np.zeros(self.XK.shape)
         WIc = np.zeros(self.WI.shape)
         for j in range(self.Ns):
             # draw x(j) from the prior
             # draw process noise according to the process noise model
             vki = self.processNoiseSample(self.XK[:,ipl[j]])
             # propagate the particle
             XKc[:,j] = self.propagateParticle(self.XK[:,ipl[j]],dt,vki)
             # assign weights
             try:
                 WIc[k] = self.measurementNoisePdf(ymeas,XKc[:,j])/self.measurementNoisePdf(ymeas,MUK[:,ipl[j]])
             except ZeroDivisionError:
                 print("Error: divide by zero in particle weight computation. Probably particle impoverishment; try more particles or new characterization function")
                 return
         # replace the prior by the updated particles and weights
         self.XK = XKc.copy()
         self.WI = WIc.copy()
         # normalize the weights
         weightFactor = 1.0/np.sum(self.WI)
         for k in range(self.Ns):
             self.WI[k] = self.WI[k]*weightFactor
     else:
         print("Error: uninitialized filter called")
Exemplo n.º 2
0
 def sample(self):
     # resample
     (self.XK,self.WI,unusedv) = pu.resample(self.XK,self.WI)