加速度センサーから変位を求める

AppleWatchの加速度センサーの値から積分を二回行い変位を求めたい

AppleWatchの加速度センサーの値から台形積分法を二回行い変位を求め，オブジェクトの座標を移動させようとしています．
しかしながら，変位を求めてオブジェクトを移動させると，移動させたい方向に移動した後に同じ分だけ戻ってきてしまいます．
積分のやり方が間違っているのでしょうか．
数学に詳しい方がいらっしゃればお教えいただきたいです．

該当のソースコード

swift
1// ローパスフィルターの係数(これは環境によって要調整。1に近づけるほど平滑化の度合いが大きくなる)
2    let filterCoefficient:Double = 0.9
3    var lowpassValue:[Double] = Array (repeating: 0, count: 3);
4    var highpassValue:[Double] = Array (repeating: 0, count: 3);
5    
6    // 時間差分
7    let timeSpan:Double = 0.05//0.1
8    // ひとつ前の加速度
9    var oldAccel:[Double] = Array (repeating: 0, count: 3)
10    //　加速度から算出した速度
11    var speed:[Double] = Array (repeating: 0, count: 3)
12    // ひとつ前の速度
13    var oldSpeed:[Double] = Array(repeating: 0, count: 3)
14    // 速度から算出した変位
15    var difference:[Double] = Array (repeating: 0, count: 3)
16    //加速度データ
17    var dataArray2:[Double] = Array(repeating: 0, count: 3)
18    
19    func Integral2(num:Int)->Double//numでx,y,zを分けている
20    {
21        // ローパスフィルター(現在の値 = 係数 * ひとつ前の値 ＋ (1 - 係数) * センサの値)
22        lowpassValue[num] = lowpassValue[num] * filterCoefficient + dataArray2[num] * (1 - filterCoefficient)
23        // ハイパスフィルター(センサの値 - ローパスフィルターの値)
24        highpassValue[num] = dataArray2[num] - lowpassValue[num];
25        
26        //速度計算
27        speed[num] = ((highpassValue[num] + oldAccel[num]) * timeSpan) / 2 + speed[num]
28        oldAccel[num] = highpassValue[num]
29        
30        //変位計算(速度を台形積分する)
31        difference[num] = ((speed[num] + oldSpeed[num]) * timeSpan) / 2 + difference[num]
32        oldSpeed[num] = speed[num];
33        
34        return difference[num]
35    }