title: Kalman filter implementation in python for speed estimation
body: <p>I try to implement Kalman filter for predicting speed one step ahead.
  Implementing in python
       H=np.diag([1,1])
        H</p>
  
  <p>Result: 
  array([[1, 0],
         [0, 1]])
  For measurement vector
  datafile is csv file containing time as one column and speed in another column</p>
  
  <pre><code>measurements=np.vstack((mx,my,datafile.speed))
   #length of meassurement
   m=measurements.shape[1]
   print(measurements.shape)
  </code></pre>
  
  <p>Output: (3, 1069)</p>
  
  <h1>Kalman</h1>
  
  <pre><code> for filterstep in range(m-1):
           #Time Update
             #=============================
           #Project the state ahead
          x=A*x
  
          #Project the error covariance ahead
          P=A*P*A.T+Q
  
          #Measurement Update(correction)
          #===================================
          #if there is GPS measurement
          if GPS[filterstep]:
          #COmpute the Kalman Gain
          S =(H*P*H).T + R
          S_inv=S.inv()
          K=(P*H.T)*S_inv
  
          #Update the estimate via z
          Z = measurements[:,filterstep].reshape(H.shape[0],1)
          y=Z-(H*x)
          x = x + (K*y)
  
          #Update the error covariance
          P=(I-(K*H))*P
  
  
  # Save states for Plotting
      x0.append(float(x[0]))
      x1.append(float(x[1]))
  
  
      Zx.append(float(Z[0]))
      Zy.append(float(Z[1]))
  
      Px.append(float(P[0,0]))
      Py.append(float(P[1,1]))
  
  
  
      Kx.append(float(K[0,0]))
      Ky.append(float(K[1,0]))
  </code></pre>
  
  <p>Error comes as:</p>
  
  <pre><code>---------------------------------------------------------------------------
  ValueError                                Traceback (most recent call last)
  &lt;ipython-input-80-9b15fccbaca8&gt; in &lt;module&gt;()
       20 
       21         #Update the estimate via z
  ---&gt; 22         Z = measurements[:,filterstep].reshape(H.shape[0],1)
       23         y=Z-(H*x)
       24         x = x + (K*y)
  
  ValueError: total size of new array must be unchanged
  </code></pre>
  
  <p>How can i remove such error </p>