hci_kalmanfilter.md

Signal processing

Tracko Model final writing

In our model, we define the relative coordination of the remote phone as the state model: $$ S_k = [P_x, P_y, P_z]^T$$

Besides, we define the movements inferred from IMU as the control model. $$ u_k = [\Delta x, \Delta y, \Delta z]^T$$

Extended Kalman Filfter v2

wolfram:

wolfram:

Extended Kalman filter for Tracko

wolfram:

wolfram:

IF $$$ \cos \phi > 0 $$$:

wolfram:

IF $$$ \cos \phi < 0 $$$:

wolfram:

Extended Kalman filter code

java implementation Extended Kalman Filter tutorial

##Kalman filter

Best tutorials: A practical approach to Kalman filter and how to implement it, An Engineer's perspective Matlab implementation

Best tutorial code: Kalman filter in python

Running processing introduction: Kalman Filter For Dummies

** Steps to work on a Kalman filter problem **

1. Define the state model, input-control model.
2. Define the transition model and observer model.
3. Recursive processing pipeline:
   • making a prediction of the future
   • getting a measurement from reality
   • comparing the two
   • moderating this difference
   • adjusting its estimate with this moderated value.

###Sensor fusion with Kalman filter ** indirect feedback kalman filter (error-state Kalman filter) **:

In such a configuration, the Kalman filter is used to estimate the difference between the current inertial and optical (or acoustic) outputs, i.e. it continually estimates the error in the inertial estimates by using the optical system as a second (redundant) reference.

This error estimate is then used to correct the inertial estimates. The tuning of the Kalman filter parameters (see “Parameter Estimation or Tuning” on page 35) then adjusts the weight of the correction as a function of frequency. Reference

Related work: Sensor Fusion & Kalman Filter 1 Sensor Fusion & Kalman Filter 2

###Example

For normal mobile phones, we have two sets of sensors: accelerometers and position sensors (like GPS, RSSI). Using the Kalman Filter can fust the sensor data and estimate the position.

** System definition **

$$ p_{t+1} = p_t + v_t \cdot {\Delta t} + \frac{1}{2} \cdot a_t \cdot {\Delta t}^2 $$

$$ v_{t+1} = v_t + a_t \cdot {\Delta t} $$

** State vector at time k is **:

$$ s_k = [p_x, p_y, p_z, v_x, v_y, v_z]^T $$

** Input-control model **:

$$ u_k = [a_x, a_y, a_z]^T $$

** Transition model **:

$$s_{k+1} = A \cdot s_k + B \cdot {u_k} + w_k$$

$$$ w_k $$$ is process noise which is Gaussian distributed with a zero mean and with covariance Q to the time k: $$ w_k \sim N(0, Q_k) $$

** Observation/Measurement model **

$$z_k = H \cdot s_k + v_k $$

$$$ v_k $$$ is measurement noise which is Gaussian distributed with a zero mean and with covariance Q to the time k: $$ v_k \sim N(0, R_k) $$

Details:

$$ A = \begin{bmatrix} 1 & 0 & 0 & {\Delta t} & 0 & 0\\ 0 & 1 & 0 & 0 & {\Delta t} & 0\\ 0 & 0 & 1 & 0 & 0 & {\Delta t}\\ 0 & 0 & 0 & 1 & 0 & 0\\ 0 & 0 & 0 & 0 & 1 & 0\\ 0 & 0 & 0 & 0 & 0 & 1\\ \end{bmatrix} $$

$$ B = \begin{bmatrix} \frac{1}{2}{\Delta t}^2 & 0 & 0\\ 0 & \frac{1}{2}{\Delta t}^2 & 0\\ 0 & 0 & \frac{1}{2}{\Delta t}^2\\ 1 & 0 & 0\\ 0 & 1 & 0\\ 0 & 0 & 1\\ \end{bmatrix} $$

Measurement covariance matrix: $$ R = \begin{bmatrix} {\sigma_x}^2 & 0 & 0\
0 & {\sigma_y}^2 & 0\
0 & 0 & {\sigma_z}^2 \
\end{bmatrix} $$

$$${\sigma_x}$$$ is simply the standard deviation of the sensor squared or the variance of the sensor. Here the sensor is the location sensor like GPS.

Process covariance matrix:

$$ R_v = E[(v_i - \overline{v})(v_i- \overline{v})] = \begin{bmatrix} {\sigma_x}^2 & 0 & 0\\ 0 & {\sigma_y}^2 & 0\\ 0 & 0 & {\sigma_z}^2 \\ \end{bmatrix} $$

$$ F = \frac {\delta s} {\delta v} = B $$

$$ Q = F \cdot R_v \cdot F^T = B \cdot R_v \cdot B^T = \begin{bmatrix} \frac{1}{2}{\Delta t}^2 & 0 & 0\\ 0 & \frac{1}{2}{\Delta t}^2 & 0\\ 0 & 0 & \frac{1}{2}{\Delta t}^2\\ 1 & 0 & 0\\ 0 & 1 & 0\\ 0 & 0 & 1\\ \end{bmatrix} \cdot \begin{bmatrix} {\sigma_x}^2 & 0 & 0\\ 0 & {\sigma_y}^2 & 0\\ 0 & 0 & {\sigma_z}^2 \\ \end{bmatrix} \cdot \begin{bmatrix} \frac{1}{2}{\Delta t}^2 & 0 & 0 & 1 & 0 & 0 \\ 0 & \frac{1}{2}{\Delta t}^2 & 0 & 0 & 1 & 0 \\ 0 & 0 & \frac{1}{2}{\Delta t}^2 & 0 & 0 & 1 \\ \end{bmatrix} $$

$$${\sigma_x}$$$ is $$${\sigma_{accx}}$$$ exactly. The standard derivation of accelermeters.

$$ H = \begin{bmatrix} 1 & 0 & 0\\ 0 & 1 & 0\\ 0 & 0 & 1\\ 0 & 0 & 0\\ 0 & 0 & 0\\ 0 & 0 & 0\\ \end{bmatrix} $$

wolframalpha calculation result

wolframalpha example for 2d

Implementations:

** References **

• Kalman Filters for undergrads
• A 3D State Space Formulation of a Navigation Kalman Filter for Autonomous Vehicles
• Robotutils
• the must read page for kalman filter
• Nice tutorial for applications
• Video tutorial for Kalman filter

###multiple rates kalman filter

• Multi-sensor fusion in Kalman Filter with different data rates

Match filter

###Peak detection in a time-series data

• matlab implementation
• c implementation
• Simple Algorithms for Peak Detection in Time-Series