/// <summary>
/// Calculate the image position measurement noise at this location.
/// </summary>
/// <param name="h">The image location. This is not constant across the image. It has the value of m_measurement_sd at the centre, increasing with radial distance to 2*m_measurement_sd at the corners</param>
/// <returns></returns>
public virtual MatrixFixed MeasurementNoise(Vector h)
{
// Distance of point we are considering from image centre
float distance = (h - m_centre).Magnitude();
float max_distance = m_centre.Magnitude();
float ratio = distance / max_distance; // goes from 0 to 1
float SD_image_filter_to_use = m_measurement_sd * (1.0f + ratio);
float measurement_noise_variance = SD_image_filter_to_use * SD_image_filter_to_use;
// RiRES is diagonal
MatrixFixed noise = new MatrixFixed(2,2);
noise.SetIdentity(measurement_noise_variance);
return noise;
}
/// <summary>
/// Fill noise covariance matrix Pnn: this is the covariance of
/// the noise vector (V)
/// (Omega)
/// that gets added to the state.
/// Form of this could change later, but for now assume that
/// V and Omega are independent, and that each of their components is
/// independent...
/// </summary>
/// <param name="xv"></param>
/// <param name="v"></param>
/// <param name="delta_t"></param>
public override void func_Q(Vector xv, Vector v, float delta_t)
{
float linear_velocity_noise_variance =
SD_A_component_filter * SD_A_component_filter * delta_t * delta_t;
float angular_velocity_noise_variance =
SD_alpha_component_filter * SD_alpha_component_filter * delta_t * delta_t;
// Independence means that the matrix is diagonal
MatrixFixed Pnn = new MatrixFixed(6,6);
Pnn.Fill(0.0f);
Pnn.Put(0, 0, linear_velocity_noise_variance);
Pnn.Put(1, 1, linear_velocity_noise_variance);
Pnn.Put(2, 2, linear_velocity_noise_variance);
Pnn.Put(3, 3, angular_velocity_noise_variance);
Pnn.Put(4, 4, angular_velocity_noise_variance);
Pnn.Put(5, 5, angular_velocity_noise_variance);
// Form Jacobian dxnew_by_dn
// Is like this:
// I * delta_t 0
// 0 dqnew_by_dOmega
// I 0
// 0 I
// Start by zeroing
MatrixFixed dxnew_by_dn = new MatrixFixed(13,6);
dxnew_by_dn.Fill(0.0f);
// Fill in easy bits first
MatrixFixed Temp33A = new MatrixFixed(3,3);
Temp33A.SetIdentity();
dxnew_by_dn.Update(Temp33A, 7, 0);
dxnew_by_dn.Update(Temp33A, 10, 3);
Temp33A *= delta_t;
dxnew_by_dn.Update(Temp33A, 0, 0);
// Tricky bit is dqnew_by_dOmega
// Is actually the same calculation as in func_fv...
// Since omega and Omega are additive...?
Vector3D rold = new Vector3D(0, 0, 0);
Vector3D vold = new Vector3D(0, 0, 0);
Vector3D omegaold = new Vector3D(0, 0, 0);
Quaternion qold=new Quaternion();
extract_r_q_v_omega(xv, rold, qold, vold, omegaold); // overkill but easy
// Fill in dqnew_by_domega = d(q x qwt)_by_dqwt . dqwt_by_domega
// Temp44A is d(q x qwt) by dqwt
MatrixFixed Temp44A = MatrixFixed.dq3_by_dq1(qold);
// Use function below for dqwt_by_domega
MatrixFixed Temp43A = new MatrixFixed(4,3);
dqomegadt_by_domega(omegaold, delta_t, Temp43A);
// Multiply them together
MatrixFixed Temp43B = Temp44A * Temp43A;
// And then plug into Jacobian
dxnew_by_dn.Update(Temp43B, 3, 3);
// Finally do Q = dxnew_by_dn . Pnn . dxnew_by_dnT
QxRES.Update( dxnew_by_dn * Pnn * dxnew_by_dn.Transpose() );
// cout << "QxRES" << QxRES;
}
/// <summary>
/// Form the covariance matrix Q of the process noise associated with x_v .
/// </summary>
/// <param name="xv"></param>
/// <param name="v"></param>
/// <param name="delta_t"></param>
public override void func_Q(Vector xv, Vector v, float delta_t)
{
// Fill noise covariance matrix Pnn: this is the covariance of
// the noise vector (V)
// (Omega)
// that gets added to the state.
// Form of this could change later, but for now assume that
// V and Omega are independent, and that each of their components is
// independent...
float linear_velocity_noise_variance =
SD_A_component_filter * SD_A_component_filter * delta_t * delta_t;
float angular_velocity_noise_variance =
SD_alpha_component_filter * SD_alpha_component_filter * delta_t * delta_t;
// Independence means that the matrix is diagonal
MatrixFixed Pnn = new MatrixFixed(6, 6);
Pnn.Fill(0.0f);
Pnn.Put(0, 0, linear_velocity_noise_variance);
Pnn.Put(1, 1, linear_velocity_noise_variance);
Pnn.Put(2, 2, linear_velocity_noise_variance);
Pnn.Put(3, 3, angular_velocity_noise_variance);
Pnn.Put(4, 4, angular_velocity_noise_variance);
Pnn.Put(5, 5, angular_velocity_noise_variance);
// Form Jacobian dxnew_by_dn
// Is like this:
// I * delta_t 0
// 0 dqnew_by_dOmega
// Start by zeroing
MatrixFixed dxnew_by_dn = new MatrixFixed(7, 6);
dxnew_by_dn.Fill(0.0f);
// The translation part is just I \Delta t
MatrixFixed Temp33A = new MatrixFixed(3, 3);
Temp33A.SetIdentity();
Temp33A *= delta_t;
dxnew_by_dn.Update(Temp33A, 0, 0);
// qnew = q x \Omega \Deltat
// dqnew_by_d\Omega = dqnew_by_d\Omega\Delta t . d\Omega\Delta t_by_d\Omega
// Get the first part
Vector qRXYZ = xv.Extract(4, 3);
Quaternion qold = new Quaternion();
qold.SetRXYZ(qRXYZ);
MatrixFixed Temp44A = MatrixFixed.dq3_by_dq1(qold);
// Use function below for dqwt_by_dOmega
Vector Omega = new Vector(3);
Omega.Fill(SD_alpha_component_filter);
MatrixFixed Temp43A = new MatrixFixed(4, 3);
dqomegadt_by_domega(new Vector3D(Omega), delta_t, Temp43A);
// Multiply them together
MatrixFixed Temp43B = Temp44A * Temp43A;
// And then plug into Jacobian
dxnew_by_dn.Update(Temp43B, 3, 3);
// Finally do Q = dxnew_by_dn . Pnn . dxnew_by_dnT
QxRES.Update(dxnew_by_dn * Pnn * dxnew_by_dn.Transpose());
}
/// <summary>
///
/// </summary>
/// <param name="xv">position and orientation of the camera</param>
/// <param name="u">Control vector of accelerations</param>
/// <param name="delta_t">time step in seconds</param>
public override void func_fv_and_dfv_by_dxv(Vector xv, Vector u, float delta_t)
{
Vector3D rold, vold, omegaold, rnew, vnew, omeganew;
Quaternion qold, qnew;
// Separate things out to make it clearer
rold = new Vector3D(0, 0, 0);
vold = new Vector3D(0, 0, 0);
omegaold = new Vector3D(0, 0, 0);
qold = new Quaternion();
extract_r_q_v_omega(xv, rold, qold, vold, omegaold);
Vector3D acceleration = new Vector3D(u);
// rnew = r + v * delta_t
//rnew = (Vector3D)((Point3D)rold + (Point3D)vold * delta_t);
rnew = new Vector3D(rold + vold * delta_t);
// qnew = q x q(omega * delta_t)
// Keep qwt ( = q(omega * delta_t)) for later use
Quaternion qwt = new Quaternion(omegaold * delta_t);
qnew = qold.Multiply(qwt);
// vnew = v
vnew = new Vector3D(vold + acceleration * delta_t);
// omeganew = omega
omeganew = omegaold;
// Put it all together
compose_xv(ref rnew, ref qnew, ref vnew, ref omeganew, ref fvRES);
// cout << "rold qold vold omegaold" << rold << qold
// << vold << omegaold;
// cout << "rnew qnew vnew omeganew" << rnew << qnew
// << vnew << omeganew;
// Now on to the Jacobian...
// Identity is a good place to start since overall structure is like this
// I 0 I * delta_t 0
// 0 dqnew_by_dq 0 dqnew_by_domega
// 0 0 I 0
// 0 0 0 I
dfv_by_dxvRES.SetIdentity();
// Fill in dxnew_by_dv = I * delta_t
MatrixFixed Temp33A = new MatrixFixed(3,3);
Temp33A.SetIdentity();
Temp33A *= delta_t;
dfv_by_dxvRES.Update(Temp33A, 0, 7);
// Fill in dqnew_by_dq
// qnew = qold x qwt ( = q3 = q2 x q1 in Scene/newbits.cc language)
MatrixFixed Temp44A = MatrixFixed.dq3_by_dq2(qwt); //4,4
dfv_by_dxvRES.Update(Temp44A, 3, 3);
// Fill in dqnew_by_domega = d(q x qwt)_by_dqwt . dqwt_by_domega
Temp44A = MatrixFixed.dq3_by_dq1(qold); // Temp44A is d(q x qwt) by dqwt
// Use function below for dqwt_by_domega
MatrixFixed Temp43A = new MatrixFixed(4,3);
dqomegadt_by_domega(omegaold, delta_t, Temp43A);
// Multiply them together
MatrixFixed Temp43B = Temp44A * Temp43A;
// And plug it in
dfv_by_dxvRES.Update(Temp43B, 3, 10);
// cout << "dfv_by_dxvRES" << dfv_by_dxvRES;
}
/// <summary>
/// Unpack and return unitary part Q.
/// </summary>
/// <returns></returns>
public MatrixFixed Q()
{
int m = qrdc_out_.Columns; // column-major storage
int n = qrdc_out_.Rows;
bool verbose = false;
if (Q_ == null)
{
Q_ = new MatrixFixed(m,m);
// extract Q.
if (verbose)
{
Debug.WriteLine("__FILE__: VNL::QR<T>Q()");
Debug.WriteLine("m,n = " + Convert.ToString(m) + ", " + Convert.ToString(n));
Debug.WriteLine("qr0 = [" + qrdc_out_ + "];");
Debug.WriteLine("aux = [" + qraux_ + "];");
}
Q_.SetIdentity();
MatrixFixed Q = Q_;
Vector v = new Vector(m);
v.Fill(0);
Vector w = new Vector(m);
w.Fill(0);
// Golub and vanLoan, p199. backward accumulation of householder matrices
// Householder vector k is [zeros(1,k-1) qraux_[k] qrdc_out_[k,:]]
for (int k = n-1; k >= 0; --k)
{
if (k >= m) continue;
// Make housevec v, and accumulate norm at the same time.
v[k] = qraux_[k];
float sq = (v[k] * v[k]);
for (int j = k+1; j < m; ++j)
{
v[j] = qrdc_out_[k,j];
sq += (v[j] * v[j]);
}
//if (verbose) MatlabPrint(std::cerr, v, "v");
// Premultiply emerging Q by house(v), noting that v[0..k-1] == 0.
// Q_new = (1 - (2/v'*v) v v')Q
// or Q -= (2/v'*v) v (v'Q)
if (sq > 0.0)
{
float scale = 2.0f/sq;
// w = (2/v'*v) v' Q
for (int i = k; i < m; ++i)
{
w[i] = 0.0f;
for (int j = k; j < m; ++j)
w[i] += scale * v[j] * Q[j, i];
//w[i] += scale * ComplexTraits.Conjugate(v[j]) * Q[j, i];
}
//if (verbose) VNL::MatlabPrint(std::cerr, w, "w");
// Q -= v w
for (int i = k; i < m; ++i)
for (int j = k; j < m; ++j)
Q[i,j] -= (v[i]) * (w[j]);
}
}
}
return Q_;
}