0
$\begingroup$

I have written the following code to smooth an (almost) linear function:

import numpy as np
import matplotlib.pyplot as plt



# intial parameters
n = 500
sz = (n) # size of array
t = np.linspace(0,10,n)
x = t + 2*t**2 # truth function
z = x + np.random.normal(0,7.5,size=sz) # noise

Q = 1e-4 # process variance

# allocate space for arrays
xhat=np.zeros(sz)      # a posteri estimate of x
P=np.zeros(sz)         # a posteri error estimate
xhatminus=np.zeros(sz) # a priori estimate of x
Pminus=np.zeros(sz)    # a priori error estimate
K=np.zeros(sz)         # Kalman gain

R = 0.1**2 # estimate of measurement variance


xhat[0] = 0.0
P[0] = 1.0

for i in range(1,n):

  xhatminus[i] = xhat[i-1]
  Pminus[i] = P[i-1]+Q


  K[i] = Pminus[i]/( Pminus[i]+R )
  xhat[i] = xhatminus[i]+K[i]*(z[i]-xhatminus[i])
  P[i] = (1-K[i])*Pminus[i]

plt.figure()
plt.plot(t, z,'k+',label='noisy measurements')
plt.plot(t, xhat,'b-',label='a posteri estimate')
plt.plot(t,x,color='g',label='truth value')
plt.legend()
plt.xlabel('t')
plt.ylabel('f(t)')


plt.show()

I have chosen x[0] = 0 because I know that's a good approx and P[0] = 1 because I didn't know better.

Plot of the estimate, noise and the true function:

Plot of the estimate, noise and the true function, Q = 1e-4, R = 0.1**2

I know that Kalman filters are not meant for non-linear functions like mine!

I have played around with both Q and R to try to find a better estimation of the true function.My function converges very slowly. Is there a way to find a "better" Q and R?

$\endgroup$
  • $\begingroup$ the easiest thing is to update more frequently $\endgroup$ – Stanley Pawlukiewicz Apr 7 '18 at 1:57
  • $\begingroup$ It looks like to me that you have not adjusted the parameters well. Try decreasing the process noise variance. $\endgroup$ – ZHUANG Apr 7 '18 at 3:02
  • $\begingroup$ (Ignore the previous comment) I do know much about python. Is it just one state in the code? If so, I recommend you to do Kalman filter with two states (e.g., position and velocity). If your filter is of two states, then you can try adding extra state (e.g., acceleration). This should boost you up. $\endgroup$ – ZHUANG Apr 7 '18 at 3:09
  • $\begingroup$ @StanleyPawlukiewicz Do you want me to increase n? $\endgroup$ – JimiChango Apr 7 '18 at 16:31
  • $\begingroup$ @ZHUANG i'm only applying the kalman filter on a nearly linear function (not related to velocity/position) $\endgroup$ – JimiChango Apr 7 '18 at 20:51
1
$\begingroup$

I believe ZHUANG's answer is correct, so I went searching for some source code in Python that implemented it. The closest I could find was a 2D example that uses velocity as well. The reference is at the top of the listing. I have reformatted and restructured the code to make it more readable to me and likely more efficient. I also adapted it to the OPs function.

I am a newbie to Kalman filters. I am using this as my learning tool. It can probably be improved upon greatly. I am posting it hoping that it will be helpful to the OP as well as others.

Ced


# Derived from:
# https://stackoverflow.com/questions/13901997/kalman-2d-filter-in-python
#   and
# https://dsp.stackexchange.com/questions/48343

import numpy as np
import matplotlib.pyplot as plt

#=============================================================================
def main():

#---- Set Parameters

        N = 50

        x = np.matrix( '0. 0. 0. 0.' ).T 
        P = np.matrix( np.eye( 4 ) ) * .0001000 # initial uncertainty

        R = 0.1**2

#---- Set up Helper Matrices

        F = np.matrix( '''
            1. 0. 1. 0.;
            0. 1. 0. 1.;
            0. 0. 1. 0.;
            0. 0. 0. 1.
            ''' )

        H = np.matrix( '''
            1. 0. 0. 0.;
            0. 1. 0. 0.''' )

        motion = np.matrix( '0. 0. 0. 0.' ).T

        Q = np.matrix( np.eye( 4 ) )

        m = np.matrix( '0. 0.' ).T

#---- Create the Test Case

#        true_x = np.linspace( 0.0, 10.0, N )
#        true_y = true_x**2

#        observed_x = true_x + 0.05 * np.random.random( N ) * true_x
#        observed_y = true_y + 0.05 * np.random.random( N ) * true_y

        sz = (N) # size of array

        true_x = np.linspace( 0.0, 10.0, N )
        true_y = true_x + 2 * true_x**2

        observed_x = true_x
        observed_y = true_y + np.random.normal( 0, 7.5, size=sz )

#---- Run the Sequence

        kalman_x = np.zeros( N )
        kalman_y = np.zeros( N )

        for n in range( N  ):
            m[0] = observed_x[n]
            m[1] = observed_y[n]
            x, P = kalman( x, P, m, R, motion, Q, F, H )
            kalman_x[n] = x[0]
            kalman_y[n] = x[1]

#---- Plot the Results

#        plt.plot( observed_x, observed_y, 'ro' )
#        plt.plot( kalman_x, kalman_y, 'g-' )
#        plt.show()

        plt.figure()
        plt.plot( true_x, observed_y,'k+',label='noisy measurements')
        plt.plot( true_x, kalman_y,'b-',label='a posteri estimate')
        plt.plot( true_x, true_y,color='g',label='truth value')
        plt.legend()
        plt.xlabel('t')
        plt.ylabel('f(t)')

        plt.show()

#=============================================================================
#def kalman( x, P, measurement, R, motion, Q, F, H ):
def kalman( x, P, m, R, motion, Q, F, H ):
        '''
        Parameters:
        x: initial state
        P: initial uncertainty convariance matrix
        measurement: observed position ( same shape as H * x )
        R: measurement noise ( same shape as H )
        motion: external motion added to state vector x
        Q: motion noise ( same shape as P )
        F: next state function: x_prime = F * x
        H: measurement function: position = H * x

        Return: the updated and predicted new values for ( x, P )

        See also http://en.wikipedia.org/wiki/Kalman_filter

        This version of kalman can be applied to many different situations by
        appropriately defining F and H 
        '''

#---- UPDATE x, P based on measurement m    
#            distance between measured and current position-belief

        y = m - H * x

        S = H * P * H.T + R  # residual convariance
        K = P * H.T * S.I    # Kalman gain

        x = x + K * y
        I = np.matrix( np.eye( F.shape[0] ) ) # identity matrix
        P = ( I - K * H ) * P

#---- PREDICT x, P based on motion

        x = F * x + motion
        P = F * P * F.T + Q

#---- Exit

        return x, P

#=============================================================================
main()

enter image description here

$\endgroup$
  • $\begingroup$ The implementation should be correct and you can smooth you result by increasing measurement noise level or reducing process noise level. $\endgroup$ – ZHUANG Apr 18 '18 at 9:07
0
$\begingroup$

As I mentioned in the comments, you should consider the second order Kalman filter to include the change of the first state (position), i.e., velocity. In fact, this is a common model which assumes the target is of constant velocity. You can check equations (13),(14) in this or equations (50-52) in this to get a better understanding.

$\endgroup$

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service, privacy policy and cookie policy

Not the answer you're looking for? Browse other questions tagged or ask your own question.