0
$\begingroup$

i want to use kalman filter to estimate my phone position, the measurments data is at this point just the accelerometer and the sampling rate is 3ms, i used the library pykalman, i have also wrote my own implementation of kalman filters and they both return the same results. transition matrix :

dt = 0.003
F = np.matrix([[1.0, 0.0, 0.0,dt,0.0,0.0,0.5*(dt)*(dt), 0.0, 0.0],
[0.0, 1.0, 0.0, 0.0,dt,0.0,0.0, 0.5*(dt)*(dt), 0.0],
[0.0, 0.0, 1.0, 0.0,0.0,dt,0.0, 0.0, 0.5*(dt)*(dt)],
[0.0, 0.0, 0.0,1.0,0.0,0.0,dt, 0.0, 0.0],
[0.0, 0.0, 0.0,0.0,1.0,0.0,0.0, dt, 0.0],
[0.0, 0.0, 0.0,0.0,0.0,1.0,0.0, 0.0, dt],
[0.0, 0.0, 0.0,0.0,0.0,0.0,1.0, 0.0, 0.0],
[0.0, 0.0, 0.0,0.0,0.0,0.0,0.0, 1.0, 0.0],
[0.0, 0.0, 0.0,0.0,0.0,0.0,0.0, 0.0, 1.0]])

measurment matrix

H =np.matrix([[0.0, 0.0, 0.0,0.0,0.0,0.0,1.0, 0.0, 0.0],
[0.0, 0.0, 0.0,0.0,0.0,0.0,0.0, 1.0, 0.0],
[0.0, 0.0, 0.0,0.0,0.0,0.0,0.0, 0.0, 1.0]])

measurment noise

rp = 0.02  # Noise of Position Measurement
R = np.matrix([[rp, 0.0, 0.0],
               [0.0, rp, 0.0],
               [0.0, 0.0, rp]])

initial state vector

X0 = np.matrix([0,0,0,0,0,0,measurments[0][0],measurments[0][1],measurments[0][2]])

state covariance matrix

P0 =np.matrix([[1, 0, 0, 0, 0, 0,0,0,0],
    [0, 1, 0,0, 0, 0,0,0,0],
    [0, 0, 1, 0, 0, 0,0,0,0],
    [0, 0, 0, 1, 0, 0,0,0,0],
    [0, 0, 0, 0, 1, 0,0,0,0],
    [0, 0, 0, 0, 0, 1,0,0,0],
     [0, 0, 0, 0, 0, 0,1,0,0],
     [0, 0, 0, 0, 0, 0,0,1,0],
     [0, 0, 0, 0, 0, 0,0,0,1]])

i load the accelerometer data, and then use pykalman to find x,y and z.

measurments = []
for i in range(len(df_acc)):
    measurments.append([df_acc.iloc[i]['x'],df_acc.iloc[i]['y'],df_acc.iloc[i]['z']-9.82])
n_timesteps = len(measurments)
n_dim_state = 9
filtered_state_means = np.zeros((n_timesteps, n_dim_state))
filtered_state_covariances = np.zeros((n_timesteps, n_dim_state, n_dim_state))

kf = KalmanFilter(transition_matrices = F, 
                  observation_matrices = H, 
                  observation_covariance = R, 
                  initial_state_mean = X0, 
                  initial_state_covariance = P0)

for t in range(n_timesteps):
    if t == 0:
        filtered_state_means[t] = X0
        filtered_state_covariances[t] = P0
    else:
        filtered_state_means[t], filtered_state_covariances[t] = (
        kf.filter_update(
            filtered_state_means[t-1],
            filtered_state_covariances[t-1],
            measurments[t]))

the results are shown in the picture, the data i captured with my phone while walking in a small appartment i can't have walked 1 mile lol, plus i've been walking in a straight line the other axis claim that ive walked 200 meters along x and z which is impossible. I can' t find where i made a mistake.

enter image description here

Improve this question
$\endgroup$
1
  • $\begingroup$ You've asked for where you've gone wrong in a moderately complex system that's not reproducible. Please edit your question to isolate more closely what your problem is. You may find that if you reduce the scope then you'll find your own answer. $\endgroup$
    – Tully ♦
    Commented Jun 9, 2022 at 23:15

1 Answer 1

Reset to default
2
$\begingroup$

You haven't made a mistake. There is a bias in your accelerometer readings (which is a true statement for all accelerometers) and then you're numerically integrating that bias, which results in position drift.

Improve this answer
$\endgroup$
1
  • 1
    $\begingroup$ It can also be noted that just using accelerations yields an unobservable model, for which any type of observer/estimator/filter can not guarantee stability/convergence. $\endgroup$
    – fibonatic
    Commented Jun 10, 2022 at 19:24

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.

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