// 3. Kalman Gain, K
Vector6d Ekf::KIMU(Matrix6d cov, Matrix16 H, double R){
// Find Kalman Gain
// Must create a 1x1 matrix container for the double RIMU_
MatrixXd RMatrix(1,1);
RMatrix << R;
Vector6d K;
K = cov*H.transpose()*(H*cov*H.transpose()+RMatrix).inverse();
return K;
}
/**
* \return The inverse matrix of this matrix \f$A^{-1}\f$ or
* an empty matrix if this matrix is not invertible.
*/
RMatrix RMatrix::getInverse() const {
if (cols != rows) {
return RMatrix();
}
RMatrix a = getAppended(createIdentity(cols));
if (!a.rref()) {
return RMatrix();
}
RMatrix ret(rows, cols);
for (int r = 0; r < rows; r++) {
for (int c = 0; c < cols; c++) {
ret.set(r, c, a.get(r, c + cols));
}
}
return ret;
}
mat4 Transform::lookAt(vec3 eye, vec3 up) {
mat4 M ;
vec3 W = glm::normalize(eye);
vec3 U = glm::normalize(glm::cross(up,W));
vec3 V = glm::normalize(glm::cross(W,U));
mat4 RMatrix (U[0], U[1], U[2], 0, V[0], V[1], V[2], 0, W[0], W[1], W[2], 0, 0, 0, 0, 1);
mat4 TMatrix (1, 0, 0, -eye[0], 0, 1, 0, -eye[1], 0, 0, 1, -eye[2], 0, 0, 0, 1);
M = TMatrix * RMatrix;
//FILL IN YOUR CODE HERE
//You must return a row-major mat4 M that you create from this routine
return M ;
}
/**
* Appends matrix \c v to the right side of this matrix
* and returns the new matrix. This matrix is not affected.
*
* \param v the matrix to append to this matrix
*
*/
RMatrix RMatrix::getAppended(const RMatrix& v) const {
if (rows != v.rows) {
return RMatrix();
}
RMatrix r(rows, cols + v.cols);
for (int rc = 0; rc < rows; ++rc) {
for (int cc = 0; cc < cols; ++cc) {
r.set(rc, cc, get(rc, cc));
}
for (int cc = cols; cc < cols + v.cols; ++cc) {
r.set(rc, cc, v.get(rc, cc - cols));
}
}
return r;
}
RTransformation RCPBody::transformation() const
{
RTransformation trans;
if (mBody) {
cpVect pos = cpBodyGetPosition(mBody);
cpVect rot = cpBodyGetRotation(mBody);
float matrix[16] = {
rot.x, rot.y, 0.0f, 0.0f,
-rot.y, rot.x, 0.0f, 0.0f,
0.0f, 0.0f, 1.0f, 0.0f,
pos.x, pos.y, 0.0f, 1.0f};
trans = RMatrix(matrix);
}
return trans;
}
void GameObject::Draw(Shader3D & shader){
auto t = TMatrix(aabb.pos.As<float>()) * RMatrix(rotation.As<float>());
Tetra.Draw(shader,t);
}
