# Doubts in the implementation of a Kalman filter to merge position data from two identical sensor

I'm currently working on the implementation of a system for calculating the vibrations of a hand using two Leapmotion devices, these devices deliver in real time the spatial location (x,y,z) and the speed of each intersection between the bones of the hand. Both devices point to the same hand from different points of view (~70° difference), the data obtained are merged using homogeneous matrices on one of the sensors (called the "support" leapmotion), obtaining results with very low error rate. After this I must give more robustness to the data resulting from the homogeneous matrix (from "support" leapmotion) by using Kalman to merge the data obtained with the "support" and the "leader" sensors.

Two doubts arise when studying Kalman, the first is about my initial matrices for the process, I can not understand on what basis they are built, I leave what I have so far.

public class Kalman {

Inverse inv = new Inverse();
public double dt = 0.01;//0.01s
public double Wk = 0;
public double Xx = 2.0;    public double Xy = 2.0;    public double Xz = 2.0;
public double Vx = 3.0;    public double Vy = 3.0;    public double Vz = 3.0;
public double dPVx = 2.0;  public double dPVy = 2.0;  public double dPVz = 2.0;
public double dPXx = 3.0;  public double dPXy = 3.0;  public double dPXz = 3.0;
public double dX = 0.1;    public double dY = 0.1;    public double dZ = 0.1;
public double dVx = 4.0;   public double dVy = 4.0;   public double dVz = 4.0;
public double Zx = 15.0;    public double Zy = 14.0;    public double Zz = 13.0;

public double Xkant [][] = {{Xx},{Xy},{Xz},{Vx},{Vy},{Vz}};

public double Akp [][] = {{1.0, 0.0, 0.0, dt, 0.0, 0.0},
{0.0, 1.0, 0.0, 0.0, dt, 0.0},
{0.0, 0.0, 1.0, 0.0, 0.0, dt},
{0.0, 0.0, 0.0, 1.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, 1.0}}; //transition matrix

public double AkpT [][] = {{1,0,0,0,0,0},
{0,1,0,0,0,0},
{0,0,1,0,0,0},
{dt,0,0,1,0,0},
{0,dt,0,0,1,0},
{0,0,dt,0,0,1}}; //transition matrix trasposed

public double Pkant [][] = {{dPXx*dPXx,0,0,0,0,0},
{0,dPXy*dPXy,0,0,0,0},
{0,0,dPXz*dPXz,0,0,0},
{0,0,0,dPVx*dPVx,0,0},
{0,0,0,0,dPVy*dPVy,0},
{0,0,0,0,0,dPVz*dPVz}}; //Initial value of covariance of estimation error

public double Q [][] = {{dX,0,0,0,0,0},
{0,dY,0,0,0,0},
{0,0,dZ,0,0,0},
{0,0,0,dVx,0,0},
{0,0,0,0,dVy,0},
{0,0,0,0,0,dVz}}; //Covariance matrix of process noise

public double R [][] = {{dX*dX,0,0,0,0,0},
{0,dY*dY,0,0,0,0},
{0,0,dZ*dZ,0,0,0},
{0,0,0,dVx*dVx,0,0},
{0,0,0,0,dVy*dVy,0},
{0,0,0,0,0,dVz*dVz}}; //covariance matrix sensor noise

public double Z [][] = {{dX*dX,0,0,0,0,0},
{0,dY*dY,0,0,0,0},
{0,0,dZ*dZ,0,0,0},
{0,0,0,dVx*dVx,0,0},
{0,0,0,0,dVy*dVy,0},
{0,0,0,0,0,dVz*dVz}}; //

public double H [][] = {{1,0,0,0,0,0},
{0,1,0,0,0,0},
{0,0,1,0,0,0}}; //just format for results?

public double HT [][] = {{1,0,0},
{0,1,0},
{0,0,1},
{0,0,0},
{0,0,0},
{0,0,0}}; //just format for results?


The second doubt is where should I insert the data of my Leapmotion "leader" and the rotated data of the Leapmotion device "support" in the Kalman equations for its correct fusion?

In some documents I could observe that matrix Xk* is 3x1 dimensions where the three variables (x, y, z) obtained by the sensors are inserted. So, where is inserted the value of the "leader" sensor? is it my Xk0? or is the data in Xk* the average of the data obtained by both sensors? or maybe I should extend Xk* so that it is 6x1 like ([Xa], [Ya], [Za], [Xb], [Yb], [Zb])?