76 lines
1.9 KiB
Go
76 lines
1.9 KiB
Go
// Copyright 2017 Google Inc.
|
|
//
|
|
// Licensed under the Apache License, Version 2.0 (the "License");
|
|
// you may not use this file except in compliance with the License.
|
|
// You may obtain a copy of the License at
|
|
//
|
|
// http://www.apache.org/licenses/LICENSE-2.0
|
|
//
|
|
// Unless required by applicable law or agreed to in writing, software
|
|
// distributed under the License is distributed on an "AS IS" BASIS,
|
|
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
|
// See the License for the specific language governing permissions and
|
|
// limitations under the License.
|
|
//
|
|
|
|
package pipoint
|
|
|
|
// Lowpass is a first order low pass filter.
|
|
type Lowpass struct {
|
|
// The current filter output.
|
|
Acc float64
|
|
}
|
|
|
|
// StepEx feeds v into the filter and returns the filtered value. Tau
|
|
// is the filter coefficient, where 1.0 is no filtering and 0.0 is no
|
|
// pass through.
|
|
func (l *Lowpass) StepEx(v, tau float64) float64 {
|
|
l.Acc = tau*v + (1-tau)*l.Acc
|
|
return l.Acc
|
|
}
|
|
|
|
// LinPred is a velocity based linear predictive filter.
|
|
type LinPred struct {
|
|
x float64
|
|
stamp float64
|
|
updated float64
|
|
v float64
|
|
}
|
|
|
|
// SetEx is called when a new measurement arrives. x is the
|
|
// measurement, now is the local time this function was called, and
|
|
// stamp is the sensor specific time the measurement was made.
|
|
//
|
|
// The velocity is calculated based on sensor time. The prediction is
|
|
// based on local time.
|
|
func (l *LinPred) SetEx(x, now, stamp float64) {
|
|
if l.stamp == 0 {
|
|
// First run
|
|
l.v = 0
|
|
} else {
|
|
dt := stamp - l.stamp
|
|
dx := x - l.x
|
|
if dt <= 0 {
|
|
l.v = 0
|
|
} else {
|
|
l.v = dx / dt
|
|
}
|
|
}
|
|
l.x = x
|
|
l.stamp = stamp
|
|
l.updated = now
|
|
}
|
|
|
|
// GetEx returns the predicted measurement based on the given local
|
|
// time.
|
|
func (l *LinPred) GetEx(now float64) float64 {
|
|
dt := now - l.updated
|
|
if dt < 0 {
|
|
dt = 0
|
|
} else if dt > 2 {
|
|
// Clamp if the value hasn't been updated recently.
|
|
dt = 2
|
|
}
|
|
return l.x + l.v*dt
|
|
}
|