forked from NaxdyOrg/NaxGCC-FW
feat(filter): implement kalman filter
This commit is contained in:
parent
b0e1eb6309
commit
eb0ba44b1c
GPG key ID:
CC15075846BCE91B
6 changed files with 232 additions and 80 deletions
|
|
@ -11,7 +11,7 @@ use embassy_rp::{
|
|||
use packed_struct::{derive::PackedStruct, PackedStruct};
|
||||
|
||||
use crate::{
|
||||
packed_float::{PackedFloat, ToPackedFloatArray},
|
||||
helpers::{PackedFloat, ToPackedFloatArray},
|
||||
stick::{NotchStatus, NO_OF_CALIBRATION_POINTS, NO_OF_NOTCHES},
|
||||
ADDR_OFFSET, FLASH_SIZE,
|
||||
};
|
||||
|
|
|
|||
262
src/filter.rs
262
src/filter.rs
|
|
@ -1,33 +1,68 @@
|
|||
use defmt::Format;
|
||||
use libm::{fminf, powf};
|
||||
use libm::{fmaxf, fminf, powf};
|
||||
|
||||
use crate::config::ControllerConfig;
|
||||
use crate::{
|
||||
config::{ControllerConfig, StickConfig},
|
||||
helpers::XyValuePair,
|
||||
};
|
||||
|
||||
macro_rules! run_kalman_on_axis {
|
||||
($self:ident, $axis:ident, $snapback:expr, $vel_weight1:ident, $vel_weight2:ident, $old_pos_filt:ident, $filter_gains:ident, $old_vel_filt:ident, $old_pos_diff:ident, $accel:ident, $vel_smooth:ident, $stick_distance6:ident) => {
|
||||
if $snapback > 0 {
|
||||
$self.vel_filt.$axis = $vel_weight1 * $self.vel.$axis
|
||||
+ (1. - $filter_gains.vel_decay.$axis) * $vel_weight2 * $old_vel_filt.$axis
|
||||
+ $filter_gains.vel_pos_factor.$axis * $old_pos_diff.$axis;
|
||||
|
||||
let pos_weight_vel_acc = 1.
|
||||
- fminf(
|
||||
1.,
|
||||
$vel_smooth.$axis * $vel_smooth.$axis * $filter_gains.vel_thresh
|
||||
+ $accel.$axis * $accel.$axis * $filter_gains.accel_thresh,
|
||||
);
|
||||
let pos_weight1 = fmaxf(pos_weight_vel_acc, $stick_distance6);
|
||||
let pos_weight2 = 1. - pos_weight1;
|
||||
|
||||
$self.pos_filt.$axis = pos_weight1 * $self.pos.$axis
|
||||
+ pos_weight2
|
||||
* ($old_pos_filt.$axis
|
||||
+ (1. - $filter_gains.vel_damp.$axis) * $self.vel_filt.$axis)
|
||||
} else if $snapback < 0 {
|
||||
let lpf = $old_pos_filt.$axis * $filter_gains.vel_damp.$axis
|
||||
+ $self.pos.$axis * (1. - $filter_gains.vel_damp.$axis);
|
||||
|
||||
let pos_weight_vel_acc = 1.
|
||||
- fminf(
|
||||
1.,
|
||||
$vel_smooth.$axis * $vel_smooth.$axis * $filter_gains.vel_thresh
|
||||
+ $accel.$axis * $accel.$axis * $filter_gains.accel_thresh,
|
||||
);
|
||||
let pos_weight1 = fmaxf(pos_weight_vel_acc, $stick_distance6);
|
||||
let pos_weight2 = 1. - pos_weight1;
|
||||
|
||||
$self.pos_filt.$axis = pos_weight1 * $self.pos.$axis + pos_weight2 * lpf;
|
||||
} else {
|
||||
$self.pos_filt.$axis = $self.pos.$axis;
|
||||
}
|
||||
};
|
||||
}
|
||||
|
||||
/// Filter gains for 800Hz, the ones for 1000Hz are provided by `get_norm_gains`
|
||||
pub const FILTER_GAINS: FilterGains = FilterGains {
|
||||
max_stick: 100.,
|
||||
x_vel_decay: 0.1,
|
||||
y_vel_decay: 0.1,
|
||||
x_vel_pos_factor: 0.01,
|
||||
y_vel_pos_factor: 0.01,
|
||||
x_vel_damp: 0.125,
|
||||
y_vel_damp: 0.125,
|
||||
vel_decay: XyValuePair { x: 0.1, y: 0.1 },
|
||||
vel_pos_factor: XyValuePair { x: 0.01, y: 0.01 },
|
||||
vel_damp: XyValuePair { x: 0.125, y: 0.125 },
|
||||
vel_thresh: 1.,
|
||||
accel_thresh: 3.,
|
||||
x_smoothing: 0.0,
|
||||
y_smoothing: 0.0,
|
||||
c_xsmoothing: 0.0,
|
||||
c_ysmoothing: 0.0,
|
||||
smoothing: XyValuePair { x: 0.0, y: 0.0 },
|
||||
c_smoothing: XyValuePair { x: 0.0, y: 0.0 },
|
||||
};
|
||||
|
||||
#[derive(Debug, Clone, Default)]
|
||||
pub struct WaveshapingValues {
|
||||
pub old_x_pos: f32,
|
||||
pub old_y_pos: f32,
|
||||
pub old_x_vel: f32,
|
||||
pub old_y_vel: f32,
|
||||
pub old_x_out: f32,
|
||||
pub old_y_out: f32,
|
||||
pub old_pos: XyValuePair<f32>,
|
||||
pub old_vel: XyValuePair<f32>,
|
||||
pub old_out: XyValuePair<f32>,
|
||||
}
|
||||
|
||||
fn calc_waveshaping_mult(setting: u8) -> f32 {
|
||||
|
|
@ -54,17 +89,14 @@ pub struct FilterGains {
|
|||
/// filtered velocity terms
|
||||
/// how fast the filtered velocity falls off in the absence of stick movement.
|
||||
/// Probably don't touch this.
|
||||
pub x_vel_decay: f32, //0.1 default for 1.2ms timesteps, larger for bigger timesteps
|
||||
pub y_vel_decay: f32,
|
||||
pub vel_decay: XyValuePair<f32>, //0.1 default for 1.2ms timesteps, larger for bigger timesteps
|
||||
/// how much the current position disagreement impacts the filtered velocity.
|
||||
/// Probably don't touch this.
|
||||
pub x_vel_pos_factor: f32, //0.01 default for 1.2ms timesteps, larger for bigger timesteps
|
||||
pub y_vel_pos_factor: f32,
|
||||
pub vel_pos_factor: XyValuePair<f32>, //0.01 default for 1.2ms timesteps, larger for bigger timesteps
|
||||
/// how much to ignore filtered velocity when computing the new stick position.
|
||||
/// DO CHANGE THIS
|
||||
/// Higher gives shorter rise times and slower fall times (more pode, less snapback)
|
||||
pub x_vel_damp: f32, //0.125 default for 1.2ms timesteps, smaller for bigger timesteps
|
||||
pub y_vel_damp: f32,
|
||||
pub vel_damp: XyValuePair<f32>, //0.125 default for 1.2ms timesteps, smaller for bigger timesteps
|
||||
/// speed and accel thresholds below which we try to follow the stick better
|
||||
/// These may need tweaking according to how noisy the signal is
|
||||
/// If it's noisier, we may need to add additional filtering
|
||||
|
|
@ -76,11 +108,9 @@ pub struct FilterGains {
|
|||
/// This just applies a low-pass filter.
|
||||
/// The purpose is to provide delay for single-axis ledgedashes.
|
||||
/// Must be between 0 and 1. Larger = more smoothing and delay.
|
||||
pub x_smoothing: f32,
|
||||
pub y_smoothing: f32,
|
||||
pub smoothing: XyValuePair<f32>,
|
||||
/// Same thing but for C-stick
|
||||
pub c_xsmoothing: f32,
|
||||
pub c_ysmoothing: f32,
|
||||
pub c_smoothing: XyValuePair<f32>,
|
||||
}
|
||||
|
||||
impl FilterGains {
|
||||
|
|
@ -88,14 +118,14 @@ impl FilterGains {
|
|||
pub fn get_normalized_gains(&self, controller_config: &ControllerConfig) -> Self {
|
||||
let mut gains = self.clone();
|
||||
|
||||
gains.x_vel_damp = vel_damp_from_snapback(controller_config.astick_config.x_snapback);
|
||||
gains.y_vel_damp = vel_damp_from_snapback(controller_config.astick_config.y_snapback);
|
||||
gains.vel_damp.x = vel_damp_from_snapback(controller_config.astick_config.x_snapback);
|
||||
gains.vel_damp.y = vel_damp_from_snapback(controller_config.astick_config.y_snapback);
|
||||
|
||||
gains.x_smoothing = controller_config.astick_config.x_smoothing as f32 / 10.;
|
||||
gains.y_smoothing = controller_config.astick_config.y_smoothing as f32 / 10.;
|
||||
gains.smoothing.x = controller_config.astick_config.x_smoothing as f32 / 10.;
|
||||
gains.smoothing.y = controller_config.astick_config.y_smoothing as f32 / 10.;
|
||||
|
||||
gains.c_xsmoothing = controller_config.cstick_config.x_smoothing as f32 / 10.;
|
||||
gains.c_ysmoothing = controller_config.cstick_config.y_smoothing as f32 / 10.;
|
||||
gains.c_smoothing.x = controller_config.cstick_config.x_smoothing as f32 / 10.;
|
||||
gains.c_smoothing.y = controller_config.cstick_config.y_smoothing as f32 / 10.;
|
||||
|
||||
// The below is assuming the sticks to be polled at 1000Hz
|
||||
let time_factor = 1.0 / 1.2;
|
||||
|
|
@ -106,37 +136,149 @@ impl FilterGains {
|
|||
|
||||
FilterGains {
|
||||
max_stick: gains.max_stick * gains.max_stick,
|
||||
x_vel_decay: gains.x_vel_decay * time_factor,
|
||||
y_vel_decay: gains.y_vel_decay * time_factor,
|
||||
x_vel_pos_factor: gains.x_vel_pos_factor * time_factor,
|
||||
y_vel_pos_factor: gains.y_vel_pos_factor * time_factor,
|
||||
x_vel_damp: gains.x_vel_damp
|
||||
vel_decay: XyValuePair {
|
||||
x: gains.vel_decay.x * time_factor,
|
||||
y: gains.vel_decay.y * time_factor,
|
||||
},
|
||||
vel_pos_factor: XyValuePair {
|
||||
x: gains.vel_pos_factor.x * time_factor,
|
||||
y: gains.vel_pos_factor.y * time_factor,
|
||||
},
|
||||
vel_damp: XyValuePair {
|
||||
x: gains.vel_damp.x
|
||||
* match controller_config.astick_config.x_snapback {
|
||||
a if a >= 0 => time_factor,
|
||||
_ => 1.0,
|
||||
},
|
||||
y_vel_damp: gains.y_vel_damp
|
||||
y: gains.vel_damp.y
|
||||
* match controller_config.astick_config.y_snapback {
|
||||
a if a >= 0 => time_factor,
|
||||
_ => 1.0,
|
||||
},
|
||||
},
|
||||
vel_thresh,
|
||||
accel_thresh,
|
||||
x_smoothing: powf(1.0 - gains.x_smoothing, time_divisor),
|
||||
y_smoothing: powf(1.0 - gains.y_smoothing, time_divisor),
|
||||
c_xsmoothing: powf(1.0 - gains.c_xsmoothing, time_divisor),
|
||||
c_ysmoothing: powf(1.0 - gains.c_ysmoothing, time_divisor),
|
||||
smoothing: XyValuePair {
|
||||
x: powf(1.0 - gains.smoothing.x, time_divisor),
|
||||
y: powf(1.0 - gains.smoothing.y, time_divisor),
|
||||
},
|
||||
c_smoothing: XyValuePair {
|
||||
x: powf(1.0 - gains.c_smoothing.x, time_divisor),
|
||||
y: powf(1.0 - gains.c_smoothing.y, time_divisor),
|
||||
},
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
pub fn run_kalman(
|
||||
#[derive(Clone, Debug, Format, Default)]
|
||||
pub struct KalmanState {
|
||||
pos: XyValuePair<f32>,
|
||||
vel: XyValuePair<f32>,
|
||||
vel_filt: XyValuePair<f32>,
|
||||
pos_filt: XyValuePair<f32>,
|
||||
}
|
||||
|
||||
impl KalmanState {
|
||||
// runs kalman filter
|
||||
pub fn run_kalman(
|
||||
&mut self,
|
||||
x_z: f32,
|
||||
y_z: f32,
|
||||
controller_config: &ControllerConfig,
|
||||
stick_config: &StickConfig,
|
||||
filter_gains: &FilterGains,
|
||||
) -> (f32, f32) {
|
||||
todo!()
|
||||
) -> (f32, f32) {
|
||||
let old_pos = self.pos;
|
||||
let old_vel = self.vel;
|
||||
let old_vel_filt = self.vel_filt;
|
||||
let old_pos_filt = self.pos_filt;
|
||||
|
||||
self.pos.x = x_z;
|
||||
self.pos.y = y_z;
|
||||
self.vel.x = x_z - old_pos.x;
|
||||
self.vel.y = y_z - old_pos.y;
|
||||
|
||||
let vel_smooth = XyValuePair {
|
||||
x: 0.5 * (self.vel.x + old_vel.x),
|
||||
y: 0.5 * (self.vel.y + old_vel.y),
|
||||
};
|
||||
let accel = XyValuePair {
|
||||
x: self.vel.x - old_vel.x,
|
||||
y: self.vel.y - old_vel.y,
|
||||
};
|
||||
let old_pos_diff = XyValuePair {
|
||||
x: old_pos.x - old_pos_filt.x,
|
||||
y: old_pos.y - old_pos_filt.y,
|
||||
};
|
||||
|
||||
let stick_distance2 = fminf(
|
||||
filter_gains.max_stick,
|
||||
self.pos.x * self.pos.x + self.pos.y * self.pos.y,
|
||||
) / filter_gains.max_stick;
|
||||
let stick_distance6 = stick_distance2 * stick_distance2 * stick_distance2;
|
||||
|
||||
let vel_weight1 = stick_distance2;
|
||||
let vel_weight2 = 1. - vel_weight1;
|
||||
|
||||
//modified velocity to feed into our kalman filter.
|
||||
//We don't actually want an accurate model of the velocity, we want to suppress snapback without adding delay
|
||||
//term 1: weight current velocity according to r^2
|
||||
//term 2: the previous filtered velocity, weighted the opposite and also set to decay
|
||||
//term 3: a corrective factor based on the disagreement between real and filtered position
|
||||
|
||||
//the current position weight used for the filtered position is whatever is larger of
|
||||
// a) 1 minus the sum of the squares of
|
||||
// 1) the smoothed velocity divided by the velocity threshold
|
||||
// 2) the acceleration divided by the accel threshold
|
||||
// b) stick r^6
|
||||
//When the stick is moving slowly, we want to weight it highly, in order to achieve
|
||||
// quick control for inputs such as tilts. We lock out using both velocity and
|
||||
// acceleration in order to rule out snapback.
|
||||
//When the stick is near the rim, we also want instant response, and we know snapback
|
||||
// doesn't reach the rim.
|
||||
|
||||
//In calculating the filtered stick position, we have the following components
|
||||
//term 1: current position, weighted according to the above weight
|
||||
//term 2: a predicted position based on the filtered velocity and previous filtered position,
|
||||
// with the filtered velocity damped, and the overall term weighted inverse of the previous term
|
||||
//term 3: the integral error correction term
|
||||
|
||||
//But if we xSnapback or ySnapback is zero, we skip the calculation
|
||||
run_kalman_on_axis!(
|
||||
self,
|
||||
x,
|
||||
stick_config.x_snapback,
|
||||
vel_weight1,
|
||||
vel_weight2,
|
||||
old_pos_filt,
|
||||
filter_gains,
|
||||
old_vel_filt,
|
||||
old_pos_diff,
|
||||
accel,
|
||||
vel_smooth,
|
||||
stick_distance6
|
||||
);
|
||||
|
||||
run_kalman_on_axis!(
|
||||
self,
|
||||
y,
|
||||
stick_config.y_snapback,
|
||||
vel_weight1,
|
||||
vel_weight2,
|
||||
old_pos_filt,
|
||||
filter_gains,
|
||||
old_vel_filt,
|
||||
old_pos_diff,
|
||||
accel,
|
||||
vel_smooth,
|
||||
stick_distance6
|
||||
);
|
||||
|
||||
self.get_xy()
|
||||
}
|
||||
|
||||
pub fn get_xy(&self) -> (f32, f32) {
|
||||
(self.pos_filt.x, self.pos_filt.y)
|
||||
}
|
||||
}
|
||||
|
||||
/// This simulates an idealized sort of pode:
|
||||
|
|
@ -160,11 +302,11 @@ pub fn run_waveshaping(
|
|||
let x_factor = calc_waveshaping_mult(x_waveshaping);
|
||||
let y_factor = calc_waveshaping_mult(y_waveshaping);
|
||||
|
||||
let x_vel = x_pos - waveshaping_values.old_x_pos;
|
||||
let y_vel = y_pos - waveshaping_values.old_y_pos;
|
||||
let x_vel = x_pos - waveshaping_values.old_pos.x;
|
||||
let y_vel = y_pos - waveshaping_values.old_pos.y;
|
||||
|
||||
let x_vel_smooth = 0.5 * (x_vel + waveshaping_values.old_x_vel);
|
||||
let y_vel_smooth = 0.5 * (y_vel + waveshaping_values.old_y_vel);
|
||||
let x_vel_smooth = 0.5 * (x_vel + waveshaping_values.old_vel.x);
|
||||
let y_vel_smooth = 0.5 * (y_vel + waveshaping_values.old_vel.y);
|
||||
|
||||
let old_x_pos_weight = fminf(
|
||||
1.,
|
||||
|
|
@ -177,15 +319,15 @@ pub fn run_waveshaping(
|
|||
);
|
||||
let new_y_pos_weight = 1. - old_y_pos_weight;
|
||||
|
||||
let x_out = x_pos * new_x_pos_weight + waveshaping_values.old_x_out * old_x_pos_weight;
|
||||
let y_out = y_pos * new_y_pos_weight + waveshaping_values.old_y_out * old_y_pos_weight;
|
||||
let x_out = x_pos * new_x_pos_weight + waveshaping_values.old_out.x * old_x_pos_weight;
|
||||
let y_out = y_pos * new_y_pos_weight + waveshaping_values.old_out.y * old_y_pos_weight;
|
||||
|
||||
waveshaping_values.old_x_pos = x_pos;
|
||||
waveshaping_values.old_y_pos = y_pos;
|
||||
waveshaping_values.old_x_vel = x_vel_smooth;
|
||||
waveshaping_values.old_y_vel = y_vel_smooth;
|
||||
waveshaping_values.old_x_out = x_out;
|
||||
waveshaping_values.old_y_out = y_out;
|
||||
waveshaping_values.old_pos.x = x_pos;
|
||||
waveshaping_values.old_pos.y = y_pos;
|
||||
waveshaping_values.old_vel.x = x_vel_smooth;
|
||||
waveshaping_values.old_vel.y = y_vel_smooth;
|
||||
waveshaping_values.old_out.x = x_out;
|
||||
waveshaping_values.old_out.y = y_out;
|
||||
|
||||
(x_out, y_out)
|
||||
}
|
||||
|
|
|
|||
|
|
@ -46,3 +46,9 @@ impl Deref for PackedFloat {
|
|||
&self.0
|
||||
}
|
||||
}
|
||||
|
||||
#[derive(Debug, Clone, Format, Default, Copy)]
|
||||
pub struct XyValuePair<T> {
|
||||
pub x: T,
|
||||
pub y: T,
|
||||
}
|
||||
16
src/input.rs
16
src/input.rs
|
|
@ -16,7 +16,7 @@ use libm::{fmaxf, fminf};
|
|||
|
||||
use crate::{
|
||||
config::ControllerConfig,
|
||||
filter::{run_kalman, run_waveshaping, FilterGains, WaveshapingValues, FILTER_GAINS},
|
||||
filter::{run_waveshaping, FilterGains, KalmanState, WaveshapingValues, FILTER_GAINS},
|
||||
gcc_hid::GcReport,
|
||||
stick::{linearize, notch_remap, StickParams},
|
||||
FLASH_SIZE,
|
||||
|
|
@ -125,6 +125,7 @@ async fn update_stick_states<
|
|||
cstick_waveshaping_values: &mut WaveshapingValues,
|
||||
old_stick_pos: &mut StickPositions,
|
||||
raw_stick_values: &mut RawStickValues,
|
||||
kalman_state: &mut KalmanState,
|
||||
) -> StickState {
|
||||
let mut adc_count = 0u32;
|
||||
let mut ax_sum = 0u32;
|
||||
|
|
@ -203,7 +204,8 @@ async fn update_stick_states<
|
|||
raw_stick_values.cx_linearized = pos_cx;
|
||||
raw_stick_values.cy_linearized = pos_cy;
|
||||
|
||||
let (x_pos_filt, y_pos_filt) = run_kalman(x_z, y_z, controller_config, &filter_gains);
|
||||
let (x_pos_filt, y_pos_filt) =
|
||||
kalman_state.run_kalman(x_z, y_z, &controller_config.astick_config, &filter_gains);
|
||||
|
||||
let (shaped_x, shaped_y) = run_waveshaping(
|
||||
x_pos_filt,
|
||||
|
|
@ -215,9 +217,9 @@ async fn update_stick_states<
|
|||
);
|
||||
|
||||
let pos_x: f32 =
|
||||
filter_gains.x_smoothing * shaped_x + (1.0 - filter_gains.x_smoothing) * old_stick_pos.x;
|
||||
filter_gains.smoothing.x * shaped_x + (1.0 - filter_gains.smoothing.x) * old_stick_pos.x;
|
||||
let pos_y =
|
||||
filter_gains.y_smoothing * shaped_y + (1.0 - filter_gains.y_smoothing) * old_stick_pos.y;
|
||||
filter_gains.smoothing.y * shaped_y + (1.0 - filter_gains.smoothing.y) * old_stick_pos.y;
|
||||
old_stick_pos.x = pos_x;
|
||||
old_stick_pos.y = pos_y;
|
||||
|
||||
|
|
@ -235,9 +237,9 @@ async fn update_stick_states<
|
|||
old_stick_pos.cx = shaped_cx;
|
||||
old_stick_pos.cy = shaped_cy;
|
||||
|
||||
let x_weight_1 = filter_gains.c_xsmoothing;
|
||||
let x_weight_1 = filter_gains.c_smoothing.x;
|
||||
let x_weight_2 = 1.0 - x_weight_1;
|
||||
let y_weight_1 = filter_gains.c_ysmoothing;
|
||||
let y_weight_1 = filter_gains.c_smoothing.y;
|
||||
let y_weight_2 = 1.0 - y_weight_1;
|
||||
|
||||
let pos_cx_filt = x_weight_1 * shaped_cx + x_weight_2 * old_cx_pos;
|
||||
|
|
@ -401,6 +403,7 @@ pub async fn input_loop(
|
|||
let mut old_stick_pos = StickPositions::default();
|
||||
let mut cstick_waveshaping_values = WaveshapingValues::default();
|
||||
let mut controlstick_waveshaping_values = WaveshapingValues::default();
|
||||
let mut kalman_state = KalmanState::default();
|
||||
|
||||
loop {
|
||||
current_stick_state = update_stick_states(
|
||||
|
|
@ -416,6 +419,7 @@ pub async fn input_loop(
|
|||
&mut cstick_waveshaping_values,
|
||||
&mut old_stick_pos,
|
||||
&mut raw_stick_values,
|
||||
&mut kalman_state,
|
||||
)
|
||||
.await;
|
||||
|
||||
|
|
|
|||
|
|
@ -7,8 +7,8 @@
|
|||
mod config;
|
||||
mod filter;
|
||||
mod gcc_hid;
|
||||
mod helpers;
|
||||
mod input;
|
||||
mod packed_float;
|
||||
mod stick;
|
||||
|
||||
use defmt::{debug, info};
|
||||
|
|
|
|||
|
|
@ -7,8 +7,8 @@ use libm::{atan2f, cosf, fabs, roundf, sinf, sqrtf};
|
|||
|
||||
use crate::{
|
||||
config::{ControllerConfig, StickConfig, DEFAULT_NOTCH_STATUS},
|
||||
helpers::ToRegularArray,
|
||||
input::Stick,
|
||||
packed_float::ToRegularArray,
|
||||
};
|
||||
|
||||
/// fit order for the linearization
|
||||
|
|
|
|||
Loading…
Reference in a new issue