Current section
Files
Jump to
Current section
Files
lib/ekf/seven_state_matrix.ex
defmodule ViaEstimation.Ekf.SevenState do
require Logger
require ViaUtils.Constants, as: VC
defstruct ekf_state: nil,
ekf_cov: nil,
r_gps: nil,
r_heading: nil,
q_ekf: nil,
imu: nil,
origin: nil,
heading_established: false
def new(config) do
imu_config = Keyword.fetch!(config, :imu_config)
imu_type = Keyword.fetch!(imu_config, :imu_type)
imu_parameters = Keyword.fetch!(imu_config, :imu_parameters)
%ViaEstimation.Ekf.SevenState{
ekf_state: {0, 0, 0, 0, 0, 0, 0},
ekf_cov: generate_ekf_cov(config),
r_gps: generate_r_gps(config),
r_heading: generate_r_heading(config),
q_ekf: generate_q(config),
imu: apply(imu_type, :new, [imu_parameters])
}
end
def predict(state, dt_accel_gyro) do
# IO.puts("q_ekf: #{inspect(state.q_ekf)}")
imu = ViaEstimation.Imu.Mahony.update(state.imu, dt_accel_gyro)
roll_rad = imu.roll_rad
pitch_rad = imu.pitch_rad
yaw_rad = imu.yaw_rad
dt_s = dt_accel_gyro.dt_s
ax_mpss = dt_accel_gyro.ax_mpss
ay_mpss = dt_accel_gyro.ay_mpss
az_mpss = dt_accel_gyro.az_mpss
# Calculcate rbg_prime
cosphi = :math.cos(roll_rad)
sinphi = :math.sin(roll_rad)
costheta = :math.cos(pitch_rad)
sintheta = :math.sin(pitch_rad)
cospsi = :math.cos(yaw_rad)
sinpsi = :math.sin(yaw_rad)
rbgp00 = -costheta * sinpsi
rbgp01 = -sinphi * sintheta * sinpsi - cosphi * cospsi
rbgp02 = -cosphi * sintheta * sinpsi + sinphi * cospsi
rbgp10 = costheta * cospsi
rbgp11 = sinphi * sintheta * cospsi - cosphi * sinpsi
rbgp12 = cosphi * sintheta * cospsi + sinphi * sinpsi
rbgp20 = 0
rbgp21 = 0
rbgp22 = 0
# Calculate inertial accel
accel_inertial_x =
az_mpss * (sinphi * sinpsi + cosphi * cospsi * sintheta) -
ay_mpss * (cosphi * sinpsi - cospsi * sinphi * sintheta) + ax_mpss * cospsi * costheta
accel_inertial_y =
ay_mpss * (cosphi * cospsi + sinphi * sinpsi * sintheta) -
az_mpss * (cospsi * sinphi - cosphi * sinpsi * sintheta) + ax_mpss * costheta * sinpsi
accel_inertial_z =
az_mpss * cosphi * costheta - ax_mpss * sintheta + ay_mpss * costheta * sinphi
{ekfs0, ekfs1, ekfs2, ekfs3, ekfs4, ekfs5, _ekfs6} = state.ekf_state
ekf_state =
{ekfs0 + ekfs3 * dt_s, ekfs1 + ekfs4 * dt_s, ekfs2 + ekfs5 * dt_s,
ekfs3 + accel_inertial_x * dt_s, ekfs4 + accel_inertial_y * dt_s,
ekfs5 + (accel_inertial_z + VC.gravity()) * dt_s, yaw_rad}
# gps = g_prime_submatrix
gps0 = rbgp00 * ax_mpss + rbgp01 * ay_mpss + rbgp02 * az_mpss
gps1 = rbgp10 * ax_mpss + rbgp11 * ay_mpss + rbgp12 * az_mpss
gps2 = rbgp20 * ax_mpss + rbgp21 * ay_mpss + rbgp22 * az_mpss
{ekfcov00, ekfcov01, ekfcov02, ekfcov03, ekfcov04, ekfcov05, ekfcov06, ekfcov10, ekfcov11,
ekfcov12, ekfcov13, ekfcov14, ekfcov15, ekfcov16, ekfcov20, ekfcov21, ekfcov22, ekfcov23,
ekfcov24, ekfcov25, ekfcov26, ekfcov30, ekfcov31, ekfcov32, ekfcov33, ekfcov34, ekfcov35,
ekfcov36, ekfcov40, ekfcov41, ekfcov42, ekfcov43, ekfcov44, ekfcov45, ekfcov46, ekfcov50,
ekfcov51, ekfcov52, ekfcov53, ekfcov54, ekfcov55, ekfcov56, ekfcov60, ekfcov61, ekfcov62,
ekfcov63, ekfcov64, ekfcov65, ekfcov66} = state.ekf_cov
c00 = ekfcov00 + dt_s * ekfcov30
c01 = ekfcov01 + dt_s * ekfcov31
c02 = ekfcov02 + dt_s * ekfcov32
c03 = ekfcov03 + dt_s * ekfcov33
c04 = ekfcov04 + dt_s * ekfcov34
c05 = ekfcov05 + dt_s * ekfcov35
c06 = ekfcov06 + dt_s * ekfcov36
c10 = ekfcov10 + dt_s * ekfcov40
c11 = ekfcov11 + dt_s * ekfcov41
c12 = ekfcov12 + dt_s * ekfcov42
c13 = ekfcov13 + dt_s * ekfcov43
c14 = ekfcov14 + dt_s * ekfcov44
c15 = ekfcov15 + dt_s * ekfcov45
c16 = ekfcov16 + dt_s * ekfcov46
c20 = ekfcov20 + dt_s * ekfcov50
c21 = ekfcov21 + dt_s * ekfcov51
c22 = ekfcov22 + dt_s * ekfcov52
c23 = ekfcov23 + dt_s * ekfcov53
c24 = ekfcov24 + dt_s * ekfcov54
c25 = ekfcov25 + dt_s * ekfcov55
c26 = ekfcov26 + dt_s * ekfcov56
c30 = ekfcov30 + ekfcov60 * gps0
c31 = ekfcov31 + ekfcov61 * gps0
c32 = ekfcov32 + ekfcov62 * gps0
c33 = ekfcov33 + ekfcov63 * gps0
c34 = ekfcov34 + ekfcov64 * gps0
c35 = ekfcov35 + ekfcov65 * gps0
c36 = ekfcov36 + ekfcov66 * gps0
c40 = ekfcov40 + ekfcov60 * gps1
c41 = ekfcov41 + ekfcov61 * gps1
c42 = ekfcov42 + ekfcov62 * gps1
c43 = ekfcov43 + ekfcov63 * gps1
c44 = ekfcov44 + ekfcov64 * gps1
c45 = ekfcov45 + ekfcov65 * gps1
c46 = ekfcov46 + ekfcov66 * gps1
c50 = ekfcov50 + ekfcov60 * gps2
c51 = ekfcov51 + ekfcov61 * gps2
c52 = ekfcov52 + ekfcov62 * gps2
c53 = ekfcov53 + ekfcov63 * gps2
c54 = ekfcov54 + ekfcov64 * gps2
c55 = ekfcov55 + ekfcov65 * gps2
c56 = ekfcov56 + ekfcov66 * gps2
c60 = ekfcov60
c61 = ekfcov61
c62 = ekfcov62
c63 = ekfcov63
c64 = ekfcov64
c65 = ekfcov65
c66 = ekfcov66
{q00, q11, q22, q33, q44, q55, q66} = state.q_ekf
ekf_cov =
{c00 + q00 + c03 * dt_s, c01 + c04 * dt_s, c02 + c05 * dt_s, c03 + c06 * gps0,
c04 + c06 * gps1, c05 + c06 * gps2, c06, c10 + c13 * dt_s, c11 + q11 + c14 * dt_s,
c12 + c15 * dt_s, c13 + c16 * gps0, c14 + c16 * gps1, c15 + c16 * gps2, c16,
c20 + c23 * dt_s, c21 + c24 * dt_s, c22 + q22 + c25 * dt_s, c23 + c26 * gps0,
c24 + c26 * gps1, c25 + c26 * gps2, c26, c30 + c33 * dt_s, c31 + c34 * dt_s,
c32 + c35 * dt_s, c33 + q33 + c36 * gps0, c34 + c36 * gps1, c35 + c36 * gps2, c36,
c40 + c43 * dt_s, c41 + c44 * dt_s, c42 + c45 * dt_s, c43 + c46 * gps0,
c44 + q44 + c46 * gps1, c45 + c46 * gps2, c46, c50 + c53 * dt_s, c51 + c54 * dt_s,
c52 + c55 * dt_s, c53 + c56 * gps0, c54 + c56 * gps1, c55 + q55 + c56 * gps2, c56,
c60 + c63 * dt_s, c61 + c64 * dt_s, c62 + c65 * dt_s, c63 + c66 * gps0, c64 + c66 * gps1,
c65 + c66 * gps2, c66 + q66}
%{state | imu: imu, ekf_state: ekf_state, ekf_cov: ekf_cov}
end
def update_from_gps(state, position_rrm, velocity_mps) do
altitude_m = -position_rrm.altitude_m
origin =
if is_nil(state.origin) do
position_rrm |> Map.put(:altitude_m, altitude_m)
else
state.origin
end
{dx, dy} = ViaUtils.Location.dx_dy_between_points(origin, position_rrm)
dz = altitude_m - origin.altitude_m
{r00, r11, r22, r33, r44, r55} = state.r_gps
{ekfcov00, ekfcov01, ekfcov02, ekfcov03, ekfcov04, ekfcov05, ekfcov06, ekfcov10, ekfcov11,
ekfcov12, ekfcov13, ekfcov14, ekfcov15, ekfcov16, ekfcov20, ekfcov21, ekfcov22, ekfcov23,
ekfcov24, ekfcov25, ekfcov26, ekfcov30, ekfcov31, ekfcov32, ekfcov33, ekfcov34, ekfcov35,
ekfcov36, ekfcov40, ekfcov41, ekfcov42, ekfcov43, ekfcov44, ekfcov45, ekfcov46, ekfcov50,
ekfcov51, ekfcov52, ekfcov53, ekfcov54, ekfcov55, ekfcov56, ekfcov60, ekfcov61, ekfcov62,
ekfcov63, ekfcov64, ekfcov65, ekfcov66} = state.ekf_cov
# Using 1-based indexing for this, because it used to be with Matrex
m11 = ekfcov00 + r00
m12 = ekfcov01
m13 = ekfcov02
m14 = ekfcov03
m15 = ekfcov04
m16 = ekfcov05
m21 = ekfcov10
m22 = ekfcov11 + r11
m23 = ekfcov12
m24 = ekfcov13
m25 = ekfcov14
m26 = ekfcov15
m31 = ekfcov20
m32 = ekfcov21
m33 = ekfcov22 + r22
m34 = ekfcov23
m35 = ekfcov24
m36 = ekfcov25
m41 = ekfcov30
m42 = ekfcov31
m43 = ekfcov32
m44 = ekfcov33 + r33
m45 = ekfcov34
m46 = ekfcov35
m51 = ekfcov40
m52 = ekfcov41
m53 = ekfcov42
m54 = ekfcov43
m55 = ekfcov44 + r44
m56 = ekfcov45
m61 = ekfcov50
m62 = ekfcov51
m63 = ekfcov52
m64 = ekfcov53
m65 = ekfcov54
m66 = ekfcov55 + r55
a4545 = m55 * m66 - m56 * m65
a3545 = m54 * m66 - m56 * m64
a3445 = m54 * m65 - m55 * m64
a2545 = m53 * m66 - m56 * m63
a2445 = m53 * m65 - m55 * m63
a2345 = m53 * m64 - m54 * m63
a1545 = m52 * m66 - m56 * m62
a1445 = m52 * m65 - m55 * m62
a1345 = m52 * m64 - m54 * m62
a1245 = m52 * m63 - m53 * m62
a0545 = m51 * m66 - m56 * m61
a0445 = m51 * m65 - m55 * m61
a0345 = m51 * m64 - m54 * m61
a0245 = m51 * m63 - m53 * m61
a0145 = m51 * m62 - m52 * m61
a4535 = m45 * m66 - m46 * m65
a3535 = m44 * m66 - m46 * m64
a3435 = m44 * m65 - m45 * m64
a2535 = m43 * m66 - m46 * m63
a2435 = m43 * m65 - m45 * m63
a2335 = m43 * m64 - m44 * m63
a1535 = m42 * m66 - m46 * m62
a1435 = m42 * m65 - m45 * m62
a1335 = m42 * m64 - m44 * m62
a1235 = m42 * m63 - m43 * m62
a4534 = m45 * m56 - m46 * m55
a3534 = m44 * m56 - m46 * m54
a3434 = m44 * m55 - m45 * m54
a2534 = m43 * m56 - m46 * m53
a2434 = m43 * m55 - m45 * m53
a2334 = m43 * m54 - m44 * m53
a1534 = m42 * m56 - m46 * m52
a1434 = m42 * m55 - m45 * m52
a1334 = m42 * m54 - m44 * m52
a1234 = m42 * m53 - m43 * m52
a0535 = m41 * m66 - m46 * m61
a0435 = m41 * m65 - m45 * m61
a0335 = m41 * m64 - m44 * m61
a0235 = m41 * m63 - m43 * m61
a0534 = m41 * m56 - m46 * m51
a0434 = m41 * m55 - m45 * m51
a0334 = m41 * m54 - m44 * m51
a0234 = m41 * m53 - m43 * m51
a0135 = m41 * m62 - m42 * m61
a0134 = m41 * m52 - m42 * m51
b345345 = m44 * a4545 - m45 * a3545 + m46 * a3445
b245345 = m43 * a4545 - m45 * a2545 + m46 * a2445
b235345 = m43 * a3545 - m44 * a2545 + m46 * a2345
b234345 = m43 * a3445 - m44 * a2445 + m45 * a2345
b145345 = m42 * a4545 - m45 * a1545 + m46 * a1445
b135345 = m42 * a3545 - m44 * a1545 + m46 * a1345
b134345 = m42 * a3445 - m44 * a1445 + m45 * a1345
b125345 = m42 * a2545 - m43 * a1545 + m46 * a1245
b124345 = m42 * a2445 - m43 * a1445 + m45 * a1245
b123345 = m42 * a2345 - m43 * a1345 + m44 * a1245
b045345 = m41 * a4545 - m45 * a0545 + m46 * a0445
b035345 = m41 * a3545 - m44 * a0545 + m46 * a0345
b034345 = m41 * a3445 - m44 * a0445 + m45 * a0345
b025345 = m41 * a2545 - m43 * a0545 + m46 * a0245
b024345 = m41 * a2445 - m43 * a0445 + m45 * a0245
b023345 = m41 * a2345 - m43 * a0345 + m44 * a0245
b015345 = m41 * a1545 - m42 * a0545 + m46 * a0145
b014345 = m41 * a1445 - m42 * a0445 + m45 * a0145
b013345 = m41 * a1345 - m42 * a0345 + m44 * a0145
b012345 = m41 * a1245 - m42 * a0245 + m43 * a0145
b345245 = m34 * a4545 - m35 * a3545 + m36 * a3445
b245245 = m33 * a4545 - m35 * a2545 + m36 * a2445
b235245 = m33 * a3545 - m34 * a2545 + m36 * a2345
b234245 = m33 * a3445 - m34 * a2445 + m35 * a2345
b145245 = m32 * a4545 - m35 * a1545 + m36 * a1445
b135245 = m32 * a3545 - m34 * a1545 + m36 * a1345
b134245 = m32 * a3445 - m34 * a1445 + m35 * a1345
b125245 = m32 * a2545 - m33 * a1545 + m36 * a1245
b124245 = m32 * a2445 - m33 * a1445 + m35 * a1245
b123245 = m32 * a2345 - m33 * a1345 + m34 * a1245
b345235 = m34 * a4535 - m35 * a3535 + m36 * a3435
b245235 = m33 * a4535 - m35 * a2535 + m36 * a2435
b235235 = m33 * a3535 - m34 * a2535 + m36 * a2335
b234235 = m33 * a3435 - m34 * a2435 + m35 * a2335
b145235 = m32 * a4535 - m35 * a1535 + m36 * a1435
b135235 = m32 * a3535 - m34 * a1535 + m36 * a1335
b134235 = m32 * a3435 - m34 * a1435 + m35 * a1335
b125235 = m32 * a2535 - m33 * a1535 + m36 * a1235
b124235 = m32 * a2435 - m33 * a1435 + m35 * a1235
b123235 = m32 * a2335 - m33 * a1335 + m34 * a1235
b345234 = m34 * a4534 - m35 * a3534 + m36 * a3434
b245234 = m33 * a4534 - m35 * a2534 + m36 * a2434
b235234 = m33 * a3534 - m34 * a2534 + m36 * a2334
b234234 = m33 * a3434 - m34 * a2434 + m35 * a2334
b145234 = m32 * a4534 - m35 * a1534 + m36 * a1434
b135234 = m32 * a3534 - m34 * a1534 + m36 * a1334
b134234 = m32 * a3434 - m34 * a1434 + m35 * a1334
b125234 = m32 * a2534 - m33 * a1534 + m36 * a1234
b124234 = m32 * a2434 - m33 * a1434 + m35 * a1234
b123234 = m32 * a2334 - m33 * a1334 + m34 * a1234
b045245 = m31 * a4545 - m35 * a0545 + m36 * a0445
b035245 = m31 * a3545 - m34 * a0545 + m36 * a0345
b034245 = m31 * a3445 - m34 * a0445 + m35 * a0345
b025245 = m31 * a2545 - m33 * a0545 + m36 * a0245
b024245 = m31 * a2445 - m33 * a0445 + m35 * a0245
b023245 = m31 * a2345 - m33 * a0345 + m34 * a0245
b045235 = m31 * a4535 - m35 * a0535 + m36 * a0435
b035235 = m31 * a3535 - m34 * a0535 + m36 * a0335
b034235 = m31 * a3435 - m34 * a0435 + m35 * a0335
b025235 = m31 * a2535 - m33 * a0535 + m36 * a0235
b024235 = m31 * a2435 - m33 * a0435 + m35 * a0235
b023235 = m31 * a2335 - m33 * a0335 + m34 * a0235
b045234 = m31 * a4534 - m35 * a0534 + m36 * a0434
b035234 = m31 * a3534 - m34 * a0534 + m36 * a0334
b034234 = m31 * a3434 - m34 * a0434 + m35 * a0334
b025234 = m31 * a2534 - m33 * a0534 + m36 * a0234
b024234 = m31 * a2434 - m33 * a0434 + m35 * a0234
b023234 = m31 * a2334 - m33 * a0334 + m34 * a0234
b015245 = m31 * a1545 - m32 * a0545 + m36 * a0145
b014245 = m31 * a1445 - m32 * a0445 + m35 * a0145
b013245 = m31 * a1345 - m32 * a0345 + m34 * a0145
b015235 = m31 * a1535 - m32 * a0535 + m36 * a0135
b014235 = m31 * a1435 - m32 * a0435 + m35 * a0135
b013235 = m31 * a1335 - m32 * a0335 + m34 * a0135
b015234 = m31 * a1534 - m32 * a0534 + m36 * a0134
b014234 = m31 * a1434 - m32 * a0434 + m35 * a0134
b013234 = m31 * a1334 - m32 * a0334 + m34 * a0134
b012245 = m31 * a1245 - m32 * a0245 + m33 * a0145
b012235 = m31 * a1235 - m32 * a0235 + m33 * a0135
b012234 = m31 * a1234 - m32 * a0234 + m33 * a0134
c23452345 = m33 * b345345 - m34 * b245345 + m35 * b235345 - m36 * b234345
c13452345 = m32 * b345345 - m34 * b145345 + m35 * b135345 - m36 * b134345
c12452345 = m32 * b245345 - m33 * b145345 + m35 * b125345 - m36 * b124345
c12352345 = m32 * b235345 - m33 * b135345 + m34 * b125345 - m36 * b123345
c12342345 = m32 * b234345 - m33 * b134345 + m34 * b124345 - m35 * b123345
c03452345 = m31 * b345345 - m34 * b045345 + m35 * b035345 - m36 * b034345
c02452345 = m31 * b245345 - m33 * b045345 + m35 * b025345 - m36 * b024345
c02352345 = m31 * b235345 - m33 * b035345 + m34 * b025345 - m36 * b023345
c02342345 = m31 * b234345 - m33 * b034345 + m34 * b024345 - m35 * b023345
c01452345 = m31 * b145345 - m32 * b045345 + m35 * b015345 - m36 * b014345
c01352345 = m31 * b135345 - m32 * b035345 + m34 * b015345 - m36 * b013345
c01342345 = m31 * b134345 - m32 * b034345 + m34 * b014345 - m35 * b013345
c01252345 = m31 * b125345 - m32 * b025345 + m33 * b015345 - m36 * b012345
c01242345 = m31 * b124345 - m32 * b024345 + m33 * b014345 - m35 * b012345
c01232345 = m31 * b123345 - m32 * b023345 + m33 * b013345 - m34 * b012345
c23451345 = m23 * b345345 - m24 * b245345 + m25 * b235345 - m26 * b234345
c13451345 = m22 * b345345 - m24 * b145345 + m25 * b135345 - m26 * b134345
c12451345 = m22 * b245345 - m23 * b145345 + m25 * b125345 - m26 * b124345
c12351345 = m22 * b235345 - m23 * b135345 + m24 * b125345 - m26 * b123345
c12341345 = m22 * b234345 - m23 * b134345 + m24 * b124345 - m25 * b123345
c23451245 = m23 * b345245 - m24 * b245245 + m25 * b235245 - m26 * b234245
c13451245 = m22 * b345245 - m24 * b145245 + m25 * b135245 - m26 * b134245
c12451245 = m22 * b245245 - m23 * b145245 + m25 * b125245 - m26 * b124245
c12351245 = m22 * b235245 - m23 * b135245 + m24 * b125245 - m26 * b123245
c12341245 = m22 * b234245 - m23 * b134245 + m24 * b124245 - m25 * b123245
c23451235 = m23 * b345235 - m24 * b245235 + m25 * b235235 - m26 * b234235
c13451235 = m22 * b345235 - m24 * b145235 + m25 * b135235 - m26 * b134235
c12451235 = m22 * b245235 - m23 * b145235 + m25 * b125235 - m26 * b124235
c12351235 = m22 * b235235 - m23 * b135235 + m24 * b125235 - m26 * b123235
c12341235 = m22 * b234235 - m23 * b134235 + m24 * b124235 - m25 * b123235
c23451234 = m23 * b345234 - m24 * b245234 + m25 * b235234 - m26 * b234234
c13451234 = m22 * b345234 - m24 * b145234 + m25 * b135234 - m26 * b134234
c12451234 = m22 * b245234 - m23 * b145234 + m25 * b125234 - m26 * b124234
c12351234 = m22 * b235234 - m23 * b135234 + m24 * b125234 - m26 * b123234
c12341234 = m22 * b234234 - m23 * b134234 + m24 * b124234 - m25 * b123234
c03451345 = m21 * b345345 - m24 * b045345 + m25 * b035345 - m26 * b034345
c02451345 = m21 * b245345 - m23 * b045345 + m25 * b025345 - m26 * b024345
c02351345 = m21 * b235345 - m23 * b035345 + m24 * b025345 - m26 * b023345
c02341345 = m21 * b234345 - m23 * b034345 + m24 * b024345 - m25 * b023345
c03451245 = m21 * b345245 - m24 * b045245 + m25 * b035245 - m26 * b034245
c02451245 = m21 * b245245 - m23 * b045245 + m25 * b025245 - m26 * b024245
c02351245 = m21 * b235245 - m23 * b035245 + m24 * b025245 - m26 * b023245
c02341245 = m21 * b234245 - m23 * b034245 + m24 * b024245 - m25 * b023245
c03451235 = m21 * b345235 - m24 * b045235 + m25 * b035235 - m26 * b034235
c02451235 = m21 * b245235 - m23 * b045235 + m25 * b025235 - m26 * b024235
c02351235 = m21 * b235235 - m23 * b035235 + m24 * b025235 - m26 * b023235
c02341235 = m21 * b234235 - m23 * b034235 + m24 * b024235 - m25 * b023235
c03451234 = m21 * b345234 - m24 * b045234 + m25 * b035234 - m26 * b034234
c02451234 = m21 * b245234 - m23 * b045234 + m25 * b025234 - m26 * b024234
c02351234 = m21 * b235234 - m23 * b035234 + m24 * b025234 - m26 * b023234
c02341234 = m21 * b234234 - m23 * b034234 + m24 * b024234 - m25 * b023234
c01451345 = m21 * b145345 - m22 * b045345 + m25 * b015345 - m26 * b014345
c01351345 = m21 * b135345 - m22 * b035345 + m24 * b015345 - m26 * b013345
c01341345 = m21 * b134345 - m22 * b034345 + m24 * b014345 - m25 * b013345
c01451245 = m21 * b145245 - m22 * b045245 + m25 * b015245 - m26 * b014245
c01351245 = m21 * b135245 - m22 * b035245 + m24 * b015245 - m26 * b013245
c01341245 = m21 * b134245 - m22 * b034245 + m24 * b014245 - m25 * b013245
c01451235 = m21 * b145235 - m22 * b045235 + m25 * b015235 - m26 * b014235
c01351235 = m21 * b135235 - m22 * b035235 + m24 * b015235 - m26 * b013235
c01341235 = m21 * b134235 - m22 * b034235 + m24 * b014235 - m25 * b013235
c01451234 = m21 * b145234 - m22 * b045234 + m25 * b015234 - m26 * b014234
c01351234 = m21 * b135234 - m22 * b035234 + m24 * b015234 - m26 * b013234
c01341234 = m21 * b134234 - m22 * b034234 + m24 * b014234 - m25 * b013234
c01251345 = m21 * b125345 - m22 * b025345 + m23 * b015345 - m26 * b012345
c01241345 = m21 * b124345 - m22 * b024345 + m23 * b014345 - m25 * b012345
c01251245 = m21 * b125245 - m22 * b025245 + m23 * b015245 - m26 * b012245
c01241245 = m21 * b124245 - m22 * b024245 + m23 * b014245 - m25 * b012245
c01251235 = m21 * b125235 - m22 * b025235 + m23 * b015235 - m26 * b012235
c01241235 = m21 * b124235 - m22 * b024235 + m23 * b014235 - m25 * b012235
c01251234 = m21 * b125234 - m22 * b025234 + m23 * b015234 - m26 * b012234
c01241234 = m21 * b124234 - m22 * b024234 + m23 * b014234 - m25 * b012234
c01231345 = m21 * b123345 - m22 * b023345 + m23 * b013345 - m24 * b012345
c01231245 = m21 * b123245 - m22 * b023245 + m23 * b013245 - m24 * b012245
c01231235 = m21 * b123235 - m22 * b023235 + m23 * b013235 - m24 * b012235
c01231234 = m21 * b123234 - m22 * b023234 + m23 * b013234 - m24 * b012234
det =
(m11 *
(m22 * c23452345 - m23 * c13452345 + m24 * c12452345 - m25 * c12352345 +
m26 * c12342345))
|> Kernel.+(
-m12 *
(m21 * c23452345 - m23 * c03452345 + m24 * c02452345 - m25 * c02352345 +
m26 * c02342345)
)
|> Kernel.+(
m13 *
(m21 * c13452345 - m22 * c03452345 + m24 * c01452345 - m25 * c01352345 +
m26 * c01342345)
)
|> Kernel.+(
-m14 *
(m21 * c12452345 - m22 * c02452345 + m23 * c01452345 - m25 * c01252345 +
m26 * c01242345)
)
|> Kernel.+(
m15 *
(m21 * c12352345 - m22 * c02352345 + m23 * c01352345 - m24 * c01252345 +
m26 * c01232345)
)
|> Kernel.+(
-m16 *
(m21 * c12342345 - m22 * c02342345 + m23 * c01342345 - m24 * c01242345 +
m25 * c01232345)
)
det = if det != 0, do: 1 / det, else: 0
inv_mat00 =
det *
(m22 * c23452345 - m23 * c13452345 + m24 * c12452345 - m25 * c12352345 +
m26 * c12342345)
inv_mat01 =
det *
-(m12 * c23452345 - m13 * c13452345 + m14 * c12452345 - m15 * c12352345 +
m16 * c12342345)
inv_mat02 =
det *
(m12 * c23451345 - m13 * c13451345 + m14 * c12451345 - m15 * c12351345 +
m16 * c12341345)
inv_mat03 =
det *
-(m12 * c23451245 - m13 * c13451245 + m14 * c12451245 - m15 * c12351245 +
m16 * c12341245)
inv_mat04 =
det *
(m12 * c23451235 - m13 * c13451235 + m14 * c12451235 - m15 * c12351235 +
m16 * c12341235)
inv_mat05 =
det *
-(m12 * c23451234 - m13 * c13451234 + m14 * c12451234 - m15 * c12351234 +
m16 * c12341234)
inv_mat10 =
det *
-(m21 * c23452345 - m23 * c03452345 + m24 * c02452345 - m25 * c02352345 +
m26 * c02342345)
inv_mat11 =
det *
(m11 * c23452345 - m13 * c03452345 + m14 * c02452345 - m15 * c02352345 +
m16 * c02342345)
inv_mat12 =
det *
-(m11 * c23451345 - m13 * c03451345 + m14 * c02451345 - m15 * c02351345 +
m16 * c02341345)
inv_mat13 =
det *
(m11 * c23451245 - m13 * c03451245 + m14 * c02451245 - m15 * c02351245 +
m16 * c02341245)
inv_mat14 =
det *
-(m11 * c23451235 - m13 * c03451235 + m14 * c02451235 - m15 * c02351235 +
m16 * c02341235)
inv_mat15 =
det *
(m11 * c23451234 - m13 * c03451234 + m14 * c02451234 - m15 * c02351234 +
m16 * c02341234)
inv_mat20 =
det *
(m21 * c13452345 - m22 * c03452345 + m24 * c01452345 - m25 * c01352345 +
m26 * c01342345)
inv_mat21 =
det *
-(m11 * c13452345 - m12 * c03452345 + m14 * c01452345 - m15 * c01352345 +
m16 * c01342345)
inv_mat22 =
det *
(m11 * c13451345 - m12 * c03451345 + m14 * c01451345 - m15 * c01351345 +
m16 * c01341345)
inv_mat23 =
det *
-(m11 * c13451245 - m12 * c03451245 + m14 * c01451245 - m15 * c01351245 +
m16 * c01341245)
inv_mat24 =
det *
(m11 * c13451235 - m12 * c03451235 + m14 * c01451235 - m15 * c01351235 +
m16 * c01341235)
inv_mat25 =
det *
-(m11 * c13451234 - m12 * c03451234 + m14 * c01451234 - m15 * c01351234 +
m16 * c01341234)
inv_mat30 =
det *
-(m21 * c12452345 - m22 * c02452345 + m23 * c01452345 - m25 * c01252345 +
m26 * c01242345)
inv_mat31 =
det *
(m11 * c12452345 - m12 * c02452345 + m13 * c01452345 - m15 * c01252345 +
m16 * c01242345)
inv_mat32 =
det *
-(m11 * c12451345 - m12 * c02451345 + m13 * c01451345 - m15 * c01251345 +
m16 * c01241345)
inv_mat33 =
det *
(m11 * c12451245 - m12 * c02451245 + m13 * c01451245 - m15 * c01251245 +
m16 * c01241245)
inv_mat34 =
det *
-(m11 * c12451235 - m12 * c02451235 + m13 * c01451235 - m15 * c01251235 +
m16 * c01241235)
inv_mat35 =
det *
(m11 * c12451234 - m12 * c02451234 + m13 * c01451234 - m15 * c01251234 +
m16 * c01241234)
inv_mat40 =
det *
(m21 * c12352345 - m22 * c02352345 + m23 * c01352345 - m24 * c01252345 +
m26 * c01232345)
inv_mat41 =
det *
-(m11 * c12352345 - m12 * c02352345 + m13 * c01352345 - m14 * c01252345 +
m16 * c01232345)
inv_mat42 =
det *
(m11 * c12351345 - m12 * c02351345 + m13 * c01351345 - m14 * c01251345 +
m16 * c01231345)
inv_mat43 =
det *
-(m11 * c12351245 - m12 * c02351245 + m13 * c01351245 - m14 * c01251245 +
m16 * c01231245)
inv_mat44 =
det *
(m11 * c12351235 - m12 * c02351235 + m13 * c01351235 - m14 * c01251235 +
m16 * c01231235)
inv_mat45 =
det *
-(m11 * c12351234 - m12 * c02351234 + m13 * c01351234 - m14 * c01251234 +
m16 * c01231234)
inv_mat50 =
det *
-(m21 * c12342345 - m22 * c02342345 + m23 * c01342345 - m24 * c01242345 +
m25 * c01232345)
inv_mat51 =
det *
(m11 * c12342345 - m12 * c02342345 + m13 * c01342345 - m14 * c01242345 +
m15 * c01232345)
inv_mat52 =
det *
-(m11 * c12341345 - m12 * c02341345 + m13 * c01341345 - m14 * c01241345 +
m15 * c01231345)
inv_mat53 =
det *
(m11 * c12341245 - m12 * c02341245 + m13 * c01341245 - m14 * c01241245 +
m15 * c01231245)
inv_mat54 =
det *
-(m11 * c12341235 - m12 * c02341235 + m13 * c01341235 - m14 * c01241235 +
m15 * c01231235)
inv_mat55 =
det *
(m11 * c12341234 - m12 * c02341234 + m13 * c01341234 - m14 * c01241234 +
m15 * c01231234)
k00 =
ekfcov00 * inv_mat00 + ekfcov01 * inv_mat10 + ekfcov02 * inv_mat20 +
ekfcov03 * inv_mat30 + ekfcov04 * inv_mat40 + ekfcov05 * inv_mat50
k01 =
ekfcov00 * inv_mat01 + ekfcov01 * inv_mat11 + ekfcov02 * inv_mat21 +
ekfcov03 * inv_mat31 + ekfcov04 * inv_mat41 + ekfcov05 * inv_mat51
k02 =
ekfcov00 * inv_mat02 + ekfcov01 * inv_mat12 + ekfcov02 * inv_mat22 +
ekfcov03 * inv_mat32 + ekfcov04 * inv_mat42 + ekfcov05 * inv_mat52
k03 =
ekfcov00 * inv_mat03 + ekfcov01 * inv_mat13 + ekfcov02 * inv_mat23 +
ekfcov03 * inv_mat33 + ekfcov04 * inv_mat43 + ekfcov05 * inv_mat53
k04 =
ekfcov00 * inv_mat04 + ekfcov01 * inv_mat14 + ekfcov02 * inv_mat24 +
ekfcov03 * inv_mat34 + ekfcov04 * inv_mat44 + ekfcov05 * inv_mat54
k05 =
ekfcov00 * inv_mat05 + ekfcov01 * inv_mat15 + ekfcov02 * inv_mat25 +
ekfcov03 * inv_mat35 + ekfcov04 * inv_mat45 + ekfcov05 * inv_mat55
k10 =
ekfcov10 * inv_mat00 + ekfcov11 * inv_mat10 + ekfcov12 * inv_mat20 +
ekfcov13 * inv_mat30 + ekfcov14 * inv_mat40 + ekfcov15 * inv_mat50
k11 =
ekfcov10 * inv_mat01 + ekfcov11 * inv_mat11 + ekfcov12 * inv_mat21 +
ekfcov13 * inv_mat31 + ekfcov14 * inv_mat41 + ekfcov15 * inv_mat51
k12 =
ekfcov10 * inv_mat02 + ekfcov11 * inv_mat12 + ekfcov12 * inv_mat22 +
ekfcov13 * inv_mat32 + ekfcov14 * inv_mat42 + ekfcov15 * inv_mat52
k13 =
ekfcov10 * inv_mat03 + ekfcov11 * inv_mat13 + ekfcov12 * inv_mat23 +
ekfcov13 * inv_mat33 + ekfcov14 * inv_mat43 + ekfcov15 * inv_mat53
k14 =
ekfcov10 * inv_mat04 + ekfcov11 * inv_mat14 + ekfcov12 * inv_mat24 +
ekfcov13 * inv_mat34 + ekfcov14 * inv_mat44 + ekfcov15 * inv_mat54
k15 =
ekfcov10 * inv_mat05 + ekfcov11 * inv_mat15 + ekfcov12 * inv_mat25 +
ekfcov13 * inv_mat35 + ekfcov14 * inv_mat45 + ekfcov15 * inv_mat55
k20 =
ekfcov20 * inv_mat00 + ekfcov21 * inv_mat10 + ekfcov22 * inv_mat20 +
ekfcov23 * inv_mat30 + ekfcov24 * inv_mat40 + ekfcov25 * inv_mat50
k21 =
ekfcov20 * inv_mat01 + ekfcov21 * inv_mat11 + ekfcov22 * inv_mat21 +
ekfcov23 * inv_mat31 + ekfcov24 * inv_mat41 + ekfcov25 * inv_mat51
k22 =
ekfcov20 * inv_mat02 + ekfcov21 * inv_mat12 + ekfcov22 * inv_mat22 +
ekfcov23 * inv_mat32 + ekfcov24 * inv_mat42 + ekfcov25 * inv_mat52
k23 =
ekfcov20 * inv_mat03 + ekfcov21 * inv_mat13 + ekfcov22 * inv_mat23 +
ekfcov23 * inv_mat33 + ekfcov24 * inv_mat43 + ekfcov25 * inv_mat53
k24 =
ekfcov20 * inv_mat04 + ekfcov21 * inv_mat14 + ekfcov22 * inv_mat24 +
ekfcov23 * inv_mat34 + ekfcov24 * inv_mat44 + ekfcov25 * inv_mat54
k25 =
ekfcov20 * inv_mat05 + ekfcov21 * inv_mat15 + ekfcov22 * inv_mat25 +
ekfcov23 * inv_mat35 + ekfcov24 * inv_mat45 + ekfcov25 * inv_mat55
k30 =
ekfcov30 * inv_mat00 + ekfcov31 * inv_mat10 + ekfcov32 * inv_mat20 +
ekfcov33 * inv_mat30 + ekfcov34 * inv_mat40 + ekfcov35 * inv_mat50
k31 =
ekfcov30 * inv_mat01 + ekfcov31 * inv_mat11 + ekfcov32 * inv_mat21 +
ekfcov33 * inv_mat31 + ekfcov34 * inv_mat41 + ekfcov35 * inv_mat51
k32 =
ekfcov30 * inv_mat02 + ekfcov31 * inv_mat12 + ekfcov32 * inv_mat22 +
ekfcov33 * inv_mat32 + ekfcov34 * inv_mat42 + ekfcov35 * inv_mat52
k33 =
ekfcov30 * inv_mat03 + ekfcov31 * inv_mat13 + ekfcov32 * inv_mat23 +
ekfcov33 * inv_mat33 + ekfcov34 * inv_mat43 + ekfcov35 * inv_mat53
k34 =
ekfcov30 * inv_mat04 + ekfcov31 * inv_mat14 + ekfcov32 * inv_mat24 +
ekfcov33 * inv_mat34 + ekfcov34 * inv_mat44 + ekfcov35 * inv_mat54
k35 =
ekfcov30 * inv_mat05 + ekfcov31 * inv_mat15 + ekfcov32 * inv_mat25 +
ekfcov33 * inv_mat35 + ekfcov34 * inv_mat45 + ekfcov35 * inv_mat55
k40 =
ekfcov40 * inv_mat00 + ekfcov41 * inv_mat10 + ekfcov42 * inv_mat20 +
ekfcov43 * inv_mat30 + ekfcov44 * inv_mat40 + ekfcov45 * inv_mat50
k41 =
ekfcov40 * inv_mat01 + ekfcov41 * inv_mat11 + ekfcov42 * inv_mat21 +
ekfcov43 * inv_mat31 + ekfcov44 * inv_mat41 + ekfcov45 * inv_mat51
k42 =
ekfcov40 * inv_mat02 + ekfcov41 * inv_mat12 + ekfcov42 * inv_mat22 +
ekfcov43 * inv_mat32 + ekfcov44 * inv_mat42 + ekfcov45 * inv_mat52
k43 =
ekfcov40 * inv_mat03 + ekfcov41 * inv_mat13 + ekfcov42 * inv_mat23 +
ekfcov43 * inv_mat33 + ekfcov44 * inv_mat43 + ekfcov45 * inv_mat53
k44 =
ekfcov40 * inv_mat04 + ekfcov41 * inv_mat14 + ekfcov42 * inv_mat24 +
ekfcov43 * inv_mat34 + ekfcov44 * inv_mat44 + ekfcov45 * inv_mat54
k45 =
ekfcov40 * inv_mat05 + ekfcov41 * inv_mat15 + ekfcov42 * inv_mat25 +
ekfcov43 * inv_mat35 + ekfcov44 * inv_mat45 + ekfcov45 * inv_mat55
k50 =
ekfcov50 * inv_mat00 + ekfcov51 * inv_mat10 + ekfcov52 * inv_mat20 +
ekfcov53 * inv_mat30 + ekfcov54 * inv_mat40 + ekfcov55 * inv_mat50
k51 =
ekfcov50 * inv_mat01 + ekfcov51 * inv_mat11 + ekfcov52 * inv_mat21 +
ekfcov53 * inv_mat31 + ekfcov54 * inv_mat41 + ekfcov55 * inv_mat51
k52 =
ekfcov50 * inv_mat02 + ekfcov51 * inv_mat12 + ekfcov52 * inv_mat22 +
ekfcov53 * inv_mat32 + ekfcov54 * inv_mat42 + ekfcov55 * inv_mat52
k53 =
ekfcov50 * inv_mat03 + ekfcov51 * inv_mat13 + ekfcov52 * inv_mat23 +
ekfcov53 * inv_mat33 + ekfcov54 * inv_mat43 + ekfcov55 * inv_mat53
k54 =
ekfcov50 * inv_mat04 + ekfcov51 * inv_mat14 + ekfcov52 * inv_mat24 +
ekfcov53 * inv_mat34 + ekfcov54 * inv_mat44 + ekfcov55 * inv_mat54
k55 =
ekfcov50 * inv_mat05 + ekfcov51 * inv_mat15 + ekfcov52 * inv_mat25 +
ekfcov53 * inv_mat35 + ekfcov54 * inv_mat45 + ekfcov55 * inv_mat55
k60 =
ekfcov60 * inv_mat00 + ekfcov61 * inv_mat10 + ekfcov62 * inv_mat20 +
ekfcov63 * inv_mat30 + ekfcov64 * inv_mat40 + ekfcov65 * inv_mat50
k61 =
ekfcov60 * inv_mat01 + ekfcov61 * inv_mat11 + ekfcov62 * inv_mat21 +
ekfcov63 * inv_mat31 + ekfcov64 * inv_mat41 + ekfcov65 * inv_mat51
k62 =
ekfcov60 * inv_mat02 + ekfcov61 * inv_mat12 + ekfcov62 * inv_mat22 +
ekfcov63 * inv_mat32 + ekfcov64 * inv_mat42 + ekfcov65 * inv_mat52
k63 =
ekfcov60 * inv_mat03 + ekfcov61 * inv_mat13 + ekfcov62 * inv_mat23 +
ekfcov63 * inv_mat33 + ekfcov64 * inv_mat43 + ekfcov65 * inv_mat53
k64 =
ekfcov60 * inv_mat04 + ekfcov61 * inv_mat14 + ekfcov62 * inv_mat24 +
ekfcov63 * inv_mat34 + ekfcov64 * inv_mat44 + ekfcov65 * inv_mat54
k65 =
ekfcov60 * inv_mat05 + ekfcov61 * inv_mat15 + ekfcov62 * inv_mat25 +
ekfcov63 * inv_mat35 + ekfcov64 * inv_mat45 + ekfcov65 * inv_mat55
{ekf_state0, ekf_state1, ekf_state2, ekf_state3, ekf_state4, ekf_state5, ekf_state6} =
state.ekf_state
dz0 = dx - ekf_state0
dz1 = dy - ekf_state1
dz2 = dz - ekf_state2
dz3 = velocity_mps.north_mps - ekf_state3
dz4 = velocity_mps.east_mps - ekf_state4
dz5 = velocity_mps.down_mps - ekf_state5
ekf_state = {
ekf_state0 + dz0 * k00 + dz1 * k01 + dz2 * k02 + dz3 * k03 + dz4 * k04 + dz5 * k05,
ekf_state1 + dz0 * k10 + dz1 * k11 + dz2 * k12 + dz3 * k13 + dz4 * k14 + dz5 * k15,
ekf_state2 + dz0 * k20 + dz1 * k21 + dz2 * k22 + dz3 * k23 + dz4 * k24 + dz5 * k25,
ekf_state3 + dz0 * k30 + dz1 * k31 + dz2 * k32 + dz3 * k33 + dz4 * k34 + dz5 * k35,
ekf_state4 + dz0 * k40 + dz1 * k41 + dz2 * k42 + dz3 * k43 + dz4 * k44 + dz5 * k45,
ekf_state5 + dz0 * k50 + dz1 * k51 + dz2 * k52 + dz3 * k53 + dz4 * k54 + dz5 * k55,
ekf_state6 + dz0 * k60 + dz1 * k61 + dz2 * k62 + dz3 * k63 + dz4 * k64 + dz5 * k65
}
ekf_cov =
{-ekfcov10 * k01 - ekfcov20 * k02 - ekfcov30 * k03 - ekfcov40 * k04 - ekfcov50 * k05 -
ekfcov00 * (k00 - 1),
-ekfcov11 * k01 - ekfcov21 * k02 - ekfcov31 * k03 - ekfcov41 * k04 - ekfcov51 * k05 -
ekfcov01 * (k00 - 1),
-ekfcov12 * k01 - ekfcov22 * k02 - ekfcov32 * k03 - ekfcov42 * k04 - ekfcov52 * k05 -
ekfcov02 * (k00 - 1),
-ekfcov13 * k01 - ekfcov23 * k02 - ekfcov33 * k03 - ekfcov43 * k04 - ekfcov53 * k05 -
ekfcov03 * (k00 - 1),
-ekfcov14 * k01 - ekfcov24 * k02 - ekfcov34 * k03 - ekfcov44 * k04 - ekfcov54 * k05 -
ekfcov04 * (k00 - 1),
-ekfcov15 * k01 - ekfcov25 * k02 - ekfcov35 * k03 - ekfcov45 * k04 - ekfcov55 * k05 -
ekfcov05 * (k00 - 1),
-ekfcov16 * k01 - ekfcov26 * k02 - ekfcov36 * k03 - ekfcov46 * k04 - ekfcov56 * k05 -
ekfcov06 * (k00 - 1),
-ekfcov00 * k10 - ekfcov20 * k12 - ekfcov30 * k13 - ekfcov40 * k14 - ekfcov50 * k15 -
ekfcov10 * (k11 - 1),
-ekfcov01 * k10 - ekfcov21 * k12 - ekfcov31 * k13 - ekfcov41 * k14 - ekfcov51 * k15 -
ekfcov11 * (k11 - 1),
-ekfcov02 * k10 - ekfcov22 * k12 - ekfcov32 * k13 - ekfcov42 * k14 - ekfcov52 * k15 -
ekfcov12 * (k11 - 1),
-ekfcov03 * k10 - ekfcov23 * k12 - ekfcov33 * k13 - ekfcov43 * k14 - ekfcov53 * k15 -
ekfcov13 * (k11 - 1),
-ekfcov04 * k10 - ekfcov24 * k12 - ekfcov34 * k13 - ekfcov44 * k14 - ekfcov54 * k15 -
ekfcov14 * (k11 - 1),
-ekfcov05 * k10 - ekfcov25 * k12 - ekfcov35 * k13 - ekfcov45 * k14 - ekfcov55 * k15 -
ekfcov15 * (k11 - 1),
-ekfcov06 * k10 - ekfcov26 * k12 - ekfcov36 * k13 - ekfcov46 * k14 - ekfcov56 * k15 -
ekfcov16 * (k11 - 1),
-ekfcov00 * k20 - ekfcov10 * k21 - ekfcov30 * k23 - ekfcov40 * k24 - ekfcov50 * k25 -
ekfcov20 * (k22 - 1),
-ekfcov01 * k20 - ekfcov11 * k21 - ekfcov31 * k23 - ekfcov41 * k24 - ekfcov51 * k25 -
ekfcov21 * (k22 - 1),
-ekfcov02 * k20 - ekfcov12 * k21 - ekfcov32 * k23 - ekfcov42 * k24 - ekfcov52 * k25 -
ekfcov22 * (k22 - 1),
-ekfcov03 * k20 - ekfcov13 * k21 - ekfcov33 * k23 - ekfcov43 * k24 - ekfcov53 * k25 -
ekfcov23 * (k22 - 1),
-ekfcov04 * k20 - ekfcov14 * k21 - ekfcov34 * k23 - ekfcov44 * k24 - ekfcov54 * k25 -
ekfcov24 * (k22 - 1),
-ekfcov05 * k20 - ekfcov15 * k21 - ekfcov35 * k23 - ekfcov45 * k24 - ekfcov55 * k25 -
ekfcov25 * (k22 - 1),
-ekfcov06 * k20 - ekfcov16 * k21 - ekfcov36 * k23 - ekfcov46 * k24 - ekfcov56 * k25 -
ekfcov26 * (k22 - 1),
-ekfcov00 * k30 - ekfcov10 * k31 - ekfcov20 * k32 - ekfcov40 * k34 - ekfcov50 * k35 -
ekfcov30 * (k33 - 1),
-ekfcov01 * k30 - ekfcov11 * k31 - ekfcov21 * k32 - ekfcov41 * k34 - ekfcov51 * k35 -
ekfcov31 * (k33 - 1),
-ekfcov02 * k30 - ekfcov12 * k31 - ekfcov22 * k32 - ekfcov42 * k34 - ekfcov52 * k35 -
ekfcov32 * (k33 - 1),
-ekfcov03 * k30 - ekfcov13 * k31 - ekfcov23 * k32 - ekfcov43 * k34 - ekfcov53 * k35 -
ekfcov33 * (k33 - 1),
-ekfcov04 * k30 - ekfcov14 * k31 - ekfcov24 * k32 - ekfcov44 * k34 - ekfcov54 * k35 -
ekfcov34 * (k33 - 1),
-ekfcov05 * k30 - ekfcov15 * k31 - ekfcov25 * k32 - ekfcov45 * k34 - ekfcov55 * k35 -
ekfcov35 * (k33 - 1),
-ekfcov06 * k30 - ekfcov16 * k31 - ekfcov26 * k32 - ekfcov46 * k34 - ekfcov56 * k35 -
ekfcov36 * (k33 - 1),
-ekfcov00 * k40 - ekfcov10 * k41 - ekfcov20 * k42 - ekfcov30 * k43 - ekfcov50 * k45 -
ekfcov40 * (k44 - 1),
-ekfcov01 * k40 - ekfcov11 * k41 - ekfcov21 * k42 - ekfcov31 * k43 - ekfcov51 * k45 -
ekfcov41 * (k44 - 1),
-ekfcov02 * k40 - ekfcov12 * k41 - ekfcov22 * k42 - ekfcov32 * k43 - ekfcov52 * k45 -
ekfcov42 * (k44 - 1),
-ekfcov03 * k40 - ekfcov13 * k41 - ekfcov23 * k42 - ekfcov33 * k43 - ekfcov53 * k45 -
ekfcov43 * (k44 - 1),
-ekfcov04 * k40 - ekfcov14 * k41 - ekfcov24 * k42 - ekfcov34 * k43 - ekfcov54 * k45 -
ekfcov44 * (k44 - 1),
-ekfcov05 * k40 - ekfcov15 * k41 - ekfcov25 * k42 - ekfcov35 * k43 - ekfcov55 * k45 -
ekfcov45 * (k44 - 1),
-ekfcov06 * k40 - ekfcov16 * k41 - ekfcov26 * k42 - ekfcov36 * k43 - ekfcov56 * k45 -
ekfcov46 * (k44 - 1),
-ekfcov00 * k50 - ekfcov10 * k51 - ekfcov20 * k52 - ekfcov30 * k53 - ekfcov40 * k54 -
ekfcov50 * (k55 - 1),
-ekfcov01 * k50 - ekfcov11 * k51 - ekfcov21 * k52 - ekfcov31 * k53 - ekfcov41 * k54 -
ekfcov51 * (k55 - 1),
-ekfcov02 * k50 - ekfcov12 * k51 - ekfcov22 * k52 - ekfcov32 * k53 - ekfcov42 * k54 -
ekfcov52 * (k55 - 1),
-ekfcov03 * k50 - ekfcov13 * k51 - ekfcov23 * k52 - ekfcov33 * k53 - ekfcov43 * k54 -
ekfcov53 * (k55 - 1),
-ekfcov04 * k50 - ekfcov14 * k51 - ekfcov24 * k52 - ekfcov34 * k53 - ekfcov44 * k54 -
ekfcov54 * (k55 - 1),
-ekfcov05 * k50 - ekfcov15 * k51 - ekfcov25 * k52 - ekfcov35 * k53 - ekfcov45 * k54 -
ekfcov55 * (k55 - 1),
-ekfcov06 * k50 - ekfcov16 * k51 - ekfcov26 * k52 - ekfcov36 * k53 - ekfcov46 * k54 -
ekfcov56 * (k55 - 1),
ekfcov60 - ekfcov00 * k60 - ekfcov10 * k61 - ekfcov20 * k62 - ekfcov30 * k63 -
ekfcov40 * k64 - ekfcov50 * k65,
ekfcov61 - ekfcov01 * k60 - ekfcov11 * k61 - ekfcov21 * k62 - ekfcov31 * k63 -
ekfcov41 * k64 - ekfcov51 * k65,
ekfcov62 - ekfcov02 * k60 - ekfcov12 * k61 - ekfcov22 * k62 - ekfcov32 * k63 -
ekfcov42 * k64 - ekfcov52 * k65,
ekfcov63 - ekfcov03 * k60 - ekfcov13 * k61 - ekfcov23 * k62 - ekfcov33 * k63 -
ekfcov43 * k64 - ekfcov53 * k65,
ekfcov64 - ekfcov04 * k60 - ekfcov14 * k61 - ekfcov24 * k62 - ekfcov34 * k63 -
ekfcov44 * k64 - ekfcov54 * k65,
ekfcov65 - ekfcov05 * k60 - ekfcov15 * k61 - ekfcov25 * k62 - ekfcov35 * k63 -
ekfcov45 * k64 - ekfcov55 * k65,
ekfcov66 - ekfcov06 * k60 - ekfcov16 * k61 - ekfcov26 * k62 - ekfcov36 * k63 -
ekfcov46 * k64 - ekfcov56 * k65}
%{state | ekf_state: ekf_state, ekf_cov: ekf_cov, origin: origin}
end
def update_from_heading(state, heading_rad) do
if state.heading_established do
{ekf_state0, ekf_state1, ekf_state2, ekf_state3, ekf_state4, ekf_state5, ekf_state6} =
state.ekf_state
delta_z = ViaUtils.Motion.turn_left_or_right_for_correction(heading_rad - ekf_state6)
{ekfcov00, ekfcov01, ekfcov02, ekfcov03, ekfcov04, ekfcov05, ekfcov06, ekfcov10, ekfcov11,
ekfcov12, ekfcov13, ekfcov14, ekfcov15, ekfcov16, ekfcov20, ekfcov21, ekfcov22, ekfcov23,
ekfcov24, ekfcov25, ekfcov26, ekfcov30, ekfcov31, ekfcov32, ekfcov33, ekfcov34, ekfcov35,
ekfcov36, ekfcov40, ekfcov41, ekfcov42, ekfcov43, ekfcov44, ekfcov45, ekfcov46, ekfcov50,
ekfcov51, ekfcov52, ekfcov53, ekfcov54, ekfcov55, ekfcov56, ekfcov60, ekfcov61, ekfcov62,
ekfcov63, ekfcov64, ekfcov65, ekfcov66} = state.ekf_cov
{r_heading} = state.r_heading
mat_div = ekfcov66 + r_heading
inv_mat = if mat_div != 0, do: 1 / mat_div, else: 0
k00 = ekfcov06 * inv_mat
k10 = ekfcov16 * inv_mat
k20 = ekfcov26 * inv_mat
k30 = ekfcov36 * inv_mat
k40 = ekfcov46 * inv_mat
k50 = ekfcov56 * inv_mat
k60 = ekfcov66 * inv_mat
ekf_state = {
ekf_state0 + delta_z * k00,
ekf_state1 + delta_z * k10,
ekf_state2 + delta_z * k20,
ekf_state3 + delta_z * k30,
ekf_state4 + delta_z * k40,
ekf_state5 + delta_z * k50,
ekf_state6 + delta_z * k60
}
ekf_cov =
{ekfcov00 + ekfcov60 * k00, ekfcov01 + ekfcov61 * k00, ekfcov02 + ekfcov62 * k00,
ekfcov03 + ekfcov63 * k00, ekfcov04 + ekfcov64 * k00, ekfcov05 + ekfcov65 * k00,
ekfcov06 + ekfcov66 * k00, ekfcov10 + ekfcov60 * k10, ekfcov11 + ekfcov61 * k10,
ekfcov12 + ekfcov62 * k10, ekfcov13 + ekfcov63 * k10, ekfcov14 + ekfcov64 * k10,
ekfcov15 + ekfcov65 * k10, ekfcov16 + ekfcov66 * k10, ekfcov20 + ekfcov60 * k20,
ekfcov21 + ekfcov61 * k20, ekfcov22 + ekfcov62 * k20, ekfcov23 + ekfcov63 * k20,
ekfcov24 + ekfcov64 * k20, ekfcov25 + ekfcov65 * k20, ekfcov26 + ekfcov66 * k20,
ekfcov30 + ekfcov60 * k30, ekfcov31 + ekfcov61 * k30, ekfcov32 + ekfcov62 * k30,
ekfcov33 + ekfcov63 * k30, ekfcov34 + ekfcov64 * k30, ekfcov35 + ekfcov65 * k30,
ekfcov36 + ekfcov66 * k30, ekfcov40 + ekfcov60 * k40, ekfcov41 + ekfcov61 * k40,
ekfcov42 + ekfcov62 * k40, ekfcov43 + ekfcov63 * k40, ekfcov44 + ekfcov64 * k40,
ekfcov45 + ekfcov65 * k40, ekfcov46 + ekfcov66 * k40, ekfcov50 + ekfcov60 * k50,
ekfcov51 + ekfcov61 * k50, ekfcov52 + ekfcov62 * k50, ekfcov53 + ekfcov63 * k50,
ekfcov54 + ekfcov64 * k50, ekfcov55 + ekfcov65 * k50, ekfcov56 + ekfcov66 * k50,
-ekfcov60 * (k60 - 1), -ekfcov61 * (k60 - 1), -ekfcov62 * (k60 - 1),
-ekfcov63 * (k60 - 1), -ekfcov64 * (k60 - 1), -ekfcov65 * (k60 - 1),
-ekfcov66 * (k60 - 1)}
delta_yaw = delta_z * k60
imu = ViaEstimation.Imu.Utils.rotate_yaw_rad(state.imu, delta_yaw)
%{state | imu: imu, ekf_state: ekf_state, ekf_cov: ekf_cov}
else
Logger.debug("Established heading at #{ViaUtils.Format.eftb_deg(heading_rad, 2)}")
ekf_state = state.ekf_state |> Kernel.put_elem(6, heading_rad)
%{state | ekf_state: ekf_state, heading_established: true}
end
end
# g_prime*ekf_cov
def mult_77_71(a77, b71) do
{
a00,
a01,
a02,
a03,
a04,
a05,
a06,
a10,
a11,
a12,
a13,
a14,
a15,
a16,
a20,
a21,
a22,
a23,
a24,
a25,
a26,
a30,
a31,
a32,
a33,
a34,
a35,
a36,
a40,
a41,
a42,
a43,
a44,
a45,
a46,
a50,
a51,
a52,
a53,
a54,
a55,
a56,
a60,
a61,
a62,
a63,
a64,
a65,
a66
} = a77
{b00, b10, b20, b30, b40, b50, b60} = b71
{
a00 * b00 + a01 * b10 + a02 * b20 + a03 * b30 + a04 * b40 + a05 * b50 + a06 * b60,
a10 * b00 + a11 * b10 + a12 * b20 + a13 * b30 + a14 * b40 + a15 * b50 + a16 * b60,
a20 * b00 + a21 * b10 + a22 * b20 + a23 * b30 + a24 * b40 + a25 * b50 + a26 * b60,
a30 * b00 + a31 * b10 + a32 * b20 + a33 * b30 + a34 * b40 + a35 * b50 + a36 * b60,
a40 * b00 + a41 * b10 + a42 * b20 + a43 * b30 + a44 * b40 + a45 * b50 + a46 * b60,
a50 * b00 + a51 * b10 + a52 * b20 + a53 * b30 + a54 * b40 + a55 * b50 + a56 * b60,
a60 * b00 + a61 * b10 + a62 * b20 + a63 * b30 + a64 * b40 + a65 * b50 + a66 * b60
}
end
# mat_to_invert
# h_prime*ekf_cov*h_primeT + r_gps
def mult_67_77_67t_plus66(a67, b77, r66) do
{
a00,
a01,
a02,
a03,
a04,
a05,
a06,
a10,
a11,
a12,
a13,
a14,
a15,
a16,
a20,
a21,
a22,
a23,
a24,
a25,
a26,
a30,
a31,
a32,
a33,
a34,
a35,
a36,
a40,
a41,
a42,
a43,
a44,
a45,
a46,
a50,
a51,
a52,
a53,
a54,
a55,
a56
} = a67
{
b00,
b01,
b02,
b03,
b04,
b05,
b06,
b10,
b11,
b12,
b13,
b14,
b15,
b16,
b20,
b21,
b22,
b23,
b24,
b25,
b26,
b30,
b31,
b32,
b33,
b34,
b35,
b36,
b40,
b41,
b42,
b43,
b44,
b45,
b46,
b50,
b51,
b52,
b53,
b54,
b55,
b56,
b60,
b61,
b62,
b63,
b64,
b65,
b66
} = b77
{r00, r11, r22, r33, r44, r55} = r66
c00 = a00 * b00 + a01 * b10 + a02 * b20 + a03 * b30 + a04 * b40 + a05 * b50 + a06 * b60
c01 = a00 * b01 + a01 * b11 + a02 * b21 + a03 * b31 + a04 * b41 + a05 * b51 + a06 * b61
c02 = a00 * b02 + a01 * b12 + a02 * b22 + a03 * b32 + a04 * b42 + a05 * b52 + a06 * b62
c03 = a00 * b03 + a01 * b13 + a02 * b23 + a03 * b33 + a04 * b43 + a05 * b53 + a06 * b63
c04 = a00 * b04 + a01 * b14 + a02 * b24 + a03 * b34 + a04 * b44 + a05 * b54 + a06 * b64
c05 = a00 * b05 + a01 * b15 + a02 * b25 + a03 * b35 + a04 * b45 + a05 * b55 + a06 * b65
c06 = a00 * b06 + a01 * b16 + a02 * b26 + a03 * b36 + a04 * b46 + a05 * b56 + a06 * b66
c10 = a10 * b00 + a11 * b10 + a12 * b20 + a13 * b30 + a14 * b40 + a15 * b50 + a16 * b60
c11 = a10 * b01 + a11 * b11 + a12 * b21 + a13 * b31 + a14 * b41 + a15 * b51 + a16 * b61
c12 = a10 * b02 + a11 * b12 + a12 * b22 + a13 * b32 + a14 * b42 + a15 * b52 + a16 * b62
c13 = a10 * b03 + a11 * b13 + a12 * b23 + a13 * b33 + a14 * b43 + a15 * b53 + a16 * b63
c14 = a10 * b04 + a11 * b14 + a12 * b24 + a13 * b34 + a14 * b44 + a15 * b54 + a16 * b64
c15 = a10 * b05 + a11 * b15 + a12 * b25 + a13 * b35 + a14 * b45 + a15 * b55 + a16 * b65
c16 = a10 * b06 + a11 * b16 + a12 * b26 + a13 * b36 + a14 * b46 + a15 * b56 + a16 * b66
c20 = a20 * b00 + a21 * b10 + a22 * b20 + a23 * b30 + a24 * b40 + a25 * b50 + a26 * b60
c21 = a20 * b01 + a21 * b11 + a22 * b21 + a23 * b31 + a24 * b41 + a25 * b51 + a26 * b61
c22 = a20 * b02 + a21 * b12 + a22 * b22 + a23 * b32 + a24 * b42 + a25 * b52 + a26 * b62
c23 = a20 * b03 + a21 * b13 + a22 * b23 + a23 * b33 + a24 * b43 + a25 * b53 + a26 * b63
c24 = a20 * b04 + a21 * b14 + a22 * b24 + a23 * b34 + a24 * b44 + a25 * b54 + a26 * b64
c25 = a20 * b05 + a21 * b15 + a22 * b25 + a23 * b35 + a24 * b45 + a25 * b55 + a26 * b65
c26 = a20 * b06 + a21 * b16 + a22 * b26 + a23 * b36 + a24 * b46 + a25 * b56 + a26 * b66
c30 = a30 * b00 + a31 * b10 + a32 * b20 + a33 * b30 + a34 * b40 + a35 * b50 + a36 * b60
c31 = a30 * b01 + a31 * b11 + a32 * b21 + a33 * b31 + a34 * b41 + a35 * b51 + a36 * b61
c32 = a30 * b02 + a31 * b12 + a32 * b22 + a33 * b32 + a34 * b42 + a35 * b52 + a36 * b62
c33 = a30 * b03 + a31 * b13 + a32 * b23 + a33 * b33 + a34 * b43 + a35 * b53 + a36 * b63
c34 = a30 * b04 + a31 * b14 + a32 * b24 + a33 * b34 + a34 * b44 + a35 * b54 + a36 * b64
c35 = a30 * b05 + a31 * b15 + a32 * b25 + a33 * b35 + a34 * b45 + a35 * b55 + a36 * b65
c36 = a30 * b06 + a31 * b16 + a32 * b26 + a33 * b36 + a34 * b46 + a35 * b56 + a36 * b66
c40 = a40 * b00 + a41 * b10 + a42 * b20 + a43 * b30 + a44 * b40 + a45 * b50 + a46 * b60
c41 = a40 * b01 + a41 * b11 + a42 * b21 + a43 * b31 + a44 * b41 + a45 * b51 + a46 * b61
c42 = a40 * b02 + a41 * b12 + a42 * b22 + a43 * b32 + a44 * b42 + a45 * b52 + a46 * b62
c43 = a40 * b03 + a41 * b13 + a42 * b23 + a43 * b33 + a44 * b43 + a45 * b53 + a46 * b63
c44 = a40 * b04 + a41 * b14 + a42 * b24 + a43 * b34 + a44 * b44 + a45 * b54 + a46 * b64
c45 = a40 * b05 + a41 * b15 + a42 * b25 + a43 * b35 + a44 * b45 + a45 * b55 + a46 * b65
c46 = a40 * b06 + a41 * b16 + a42 * b26 + a43 * b36 + a44 * b46 + a45 * b56 + a46 * b66
c50 = a50 * b00 + a51 * b10 + a52 * b20 + a53 * b30 + a54 * b40 + a55 * b50 + a56 * b60
c51 = a50 * b01 + a51 * b11 + a52 * b21 + a53 * b31 + a54 * b41 + a55 * b51 + a56 * b61
c52 = a50 * b02 + a51 * b12 + a52 * b22 + a53 * b32 + a54 * b42 + a55 * b52 + a56 * b62
c53 = a50 * b03 + a51 * b13 + a52 * b23 + a53 * b33 + a54 * b43 + a55 * b53 + a56 * b63
c54 = a50 * b04 + a51 * b14 + a52 * b24 + a53 * b34 + a54 * b44 + a55 * b54 + a56 * b64
c55 = a50 * b05 + a51 * b15 + a52 * b25 + a53 * b35 + a54 * b45 + a55 * b55 + a56 * b65
c56 = a50 * b06 + a51 * b16 + a52 * b26 + a53 * b36 + a54 * b46 + a55 * b56 + a56 * b66
{r00 + a00 * c00 + a01 * c01 + a02 * c02 + a03 * c03 + a04 * c04 + a05 * c05 + a06 * c06,
a10 * c00 + a11 * c01 + a12 * c02 + a13 * c03 + a14 * c04 + a15 * c05 + a16 * c06,
a20 * c00 + a21 * c01 + a22 * c02 + a23 * c03 + a24 * c04 + a25 * c05 + a26 * c06,
a30 * c00 + a31 * c01 + a32 * c02 + a33 * c03 + a34 * c04 + a35 * c05 + a36 * c06,
a40 * c00 + a41 * c01 + a42 * c02 + a43 * c03 + a44 * c04 + a45 * c05 + a46 * c06,
a50 * c00 + a51 * c01 + a52 * c02 + a53 * c03 + a54 * c04 + a55 * c05 + a56 * c06,
a00 * c10 + a01 * c11 + a02 * c12 + a03 * c13 + a04 * c14 + a05 * c15 + a06 * c16,
r11 + a10 * c10 + a11 * c11 + a12 * c12 + a13 * c13 + a14 * c14 + a15 * c15 + a16 * c16,
a20 * c10 + a21 * c11 + a22 * c12 + a23 * c13 + a24 * c14 + a25 * c15 + a26 * c16,
a30 * c10 + a31 * c11 + a32 * c12 + a33 * c13 + a34 * c14 + a35 * c15 + a36 * c16,
a40 * c10 + a41 * c11 + a42 * c12 + a43 * c13 + a44 * c14 + a45 * c15 + a46 * c16,
a50 * c10 + a51 * c11 + a52 * c12 + a53 * c13 + a54 * c14 + a55 * c15 + a56 * c16,
a00 * c20 + a01 * c21 + a02 * c22 + a03 * c23 + a04 * c24 + a05 * c25 + a06 * c26,
a10 * c20 + a11 * c21 + a12 * c22 + a13 * c23 + a14 * c24 + a15 * c25 + a16 * c26,
r22 + a20 * c20 + a21 * c21 + a22 * c22 + a23 * c23 + a24 * c24 + a25 * c25 + a26 * c26,
a30 * c20 + a31 * c21 + a32 * c22 + a33 * c23 + a34 * c24 + a35 * c25 + a36 * c26,
a40 * c20 + a41 * c21 + a42 * c22 + a43 * c23 + a44 * c24 + a45 * c25 + a46 * c26,
a50 * c20 + a51 * c21 + a52 * c22 + a53 * c23 + a54 * c24 + a55 * c25 + a56 * c26,
a00 * c30 + a01 * c31 + a02 * c32 + a03 * c33 + a04 * c34 + a05 * c35 + a06 * c36,
a10 * c30 + a11 * c31 + a12 * c32 + a13 * c33 + a14 * c34 + a15 * c35 + a16 * c36,
a20 * c30 + a21 * c31 + a22 * c32 + a23 * c33 + a24 * c34 + a25 * c35 + a26 * c36,
r33 + a30 * c30 + a31 * c31 + a32 * c32 + a33 * c33 + a34 * c34 + a35 * c35 + a36 * c36,
a40 * c30 + a41 * c31 + a42 * c32 + a43 * c33 + a44 * c34 + a45 * c35 + a46 * c36,
a50 * c30 + a51 * c31 + a52 * c32 + a53 * c33 + a54 * c34 + a55 * c35 + a56 * c36,
a00 * c40 + a01 * c41 + a02 * c42 + a03 * c43 + a04 * c44 + a05 * c45 + a06 * c46,
a10 * c40 + a11 * c41 + a12 * c42 + a13 * c43 + a14 * c44 + a15 * c45 + a16 * c46,
a20 * c40 + a21 * c41 + a22 * c42 + a23 * c43 + a24 * c44 + a25 * c45 + a26 * c46,
a30 * c40 + a31 * c41 + a32 * c42 + a33 * c43 + a34 * c44 + a35 * c45 + a36 * c46,
r44 + a40 * c40 + a41 * c41 + a42 * c42 + a43 * c43 + a44 * c44 + a45 * c45 + a46 * c46,
a50 * c40 + a51 * c41 + a52 * c42 + a53 * c43 + a54 * c44 + a55 * c45 + a56 * c46,
a00 * c50 + a01 * c51 + a02 * c52 + a03 * c53 + a04 * c54 + a05 * c55 + a06 * c56,
a10 * c50 + a11 * c51 + a12 * c52 + a13 * c53 + a14 * c54 + a15 * c55 + a16 * c56,
a20 * c50 + a21 * c51 + a22 * c52 + a23 * c53 + a24 * c54 + a25 * c55 + a26 * c56,
a30 * c50 + a31 * c51 + a32 * c52 + a33 * c53 + a34 * c54 + a35 * c55 + a36 * c56,
a40 * c50 + a41 * c51 + a42 * c52 + a43 * c53 + a44 * c54 + a45 * c55 + a46 * c56,
r55 + a50 * c50 + a51 * c51 + a52 * c52 + a53 * c53 + a54 * c54 + a55 * c55 + a56 * c56}
end
def mult_77_77(a77, b77) do
{
a00,
a01,
a02,
a03,
a04,
a05,
a06,
a10,
a11,
a12,
a13,
a14,
a15,
a16,
a20,
a21,
a22,
a23,
a24,
a25,
a26,
a30,
a31,
a32,
a33,
a34,
a35,
a36,
a40,
a41,
a42,
a43,
a44,
a45,
a46,
a50,
a51,
a52,
a53,
a54,
a55,
a56,
a60,
a61,
a62,
a63,
a64,
a65,
a66
} = a77
{
b00,
b01,
b02,
b03,
b04,
b05,
b06,
b10,
b11,
b12,
b13,
b14,
b15,
b16,
b20,
b21,
b22,
b23,
b24,
b25,
b26,
b30,
b31,
b32,
b33,
b34,
b35,
b36,
b40,
b41,
b42,
b43,
b44,
b45,
b46,
b50,
b51,
b52,
b53,
b54,
b55,
b56,
b60,
b61,
b62,
b63,
b64,
b65,
b66
} = b77
{a00 * b00 + a01 * b10 + a02 * b20 + a03 * b30 + a04 * b40 + a05 * b50 + a06 * b60,
a00 * b01 + a01 * b11 + a02 * b21 + a03 * b31 + a04 * b41 + a05 * b51 + a06 * b61,
a00 * b02 + a01 * b12 + a02 * b22 + a03 * b32 + a04 * b42 + a05 * b52 + a06 * b62,
a00 * b03 + a01 * b13 + a02 * b23 + a03 * b33 + a04 * b43 + a05 * b53 + a06 * b63,
a00 * b04 + a01 * b14 + a02 * b24 + a03 * b34 + a04 * b44 + a05 * b54 + a06 * b64,
a00 * b05 + a01 * b15 + a02 * b25 + a03 * b35 + a04 * b45 + a05 * b55 + a06 * b65,
a00 * b06 + a01 * b16 + a02 * b26 + a03 * b36 + a04 * b46 + a05 * b56 + a06 * b66,
a10 * b00 + a11 * b10 + a12 * b20 + a13 * b30 + a14 * b40 + a15 * b50 + a16 * b60,
a10 * b01 + a11 * b11 + a12 * b21 + a13 * b31 + a14 * b41 + a15 * b51 + a16 * b61,
a10 * b02 + a11 * b12 + a12 * b22 + a13 * b32 + a14 * b42 + a15 * b52 + a16 * b62,
a10 * b03 + a11 * b13 + a12 * b23 + a13 * b33 + a14 * b43 + a15 * b53 + a16 * b63,
a10 * b04 + a11 * b14 + a12 * b24 + a13 * b34 + a14 * b44 + a15 * b54 + a16 * b64,
a10 * b05 + a11 * b15 + a12 * b25 + a13 * b35 + a14 * b45 + a15 * b55 + a16 * b65,
a10 * b06 + a11 * b16 + a12 * b26 + a13 * b36 + a14 * b46 + a15 * b56 + a16 * b66,
a20 * b00 + a21 * b10 + a22 * b20 + a23 * b30 + a24 * b40 + a25 * b50 + a26 * b60,
a20 * b01 + a21 * b11 + a22 * b21 + a23 * b31 + a24 * b41 + a25 * b51 + a26 * b61,
a20 * b02 + a21 * b12 + a22 * b22 + a23 * b32 + a24 * b42 + a25 * b52 + a26 * b62,
a20 * b03 + a21 * b13 + a22 * b23 + a23 * b33 + a24 * b43 + a25 * b53 + a26 * b63,
a20 * b04 + a21 * b14 + a22 * b24 + a23 * b34 + a24 * b44 + a25 * b54 + a26 * b64,
a20 * b05 + a21 * b15 + a22 * b25 + a23 * b35 + a24 * b45 + a25 * b55 + a26 * b65,
a20 * b06 + a21 * b16 + a22 * b26 + a23 * b36 + a24 * b46 + a25 * b56 + a26 * b66,
a30 * b00 + a31 * b10 + a32 * b20 + a33 * b30 + a34 * b40 + a35 * b50 + a36 * b60,
a30 * b01 + a31 * b11 + a32 * b21 + a33 * b31 + a34 * b41 + a35 * b51 + a36 * b61,
a30 * b02 + a31 * b12 + a32 * b22 + a33 * b32 + a34 * b42 + a35 * b52 + a36 * b62,
a30 * b03 + a31 * b13 + a32 * b23 + a33 * b33 + a34 * b43 + a35 * b53 + a36 * b63,
a30 * b04 + a31 * b14 + a32 * b24 + a33 * b34 + a34 * b44 + a35 * b54 + a36 * b64,
a30 * b05 + a31 * b15 + a32 * b25 + a33 * b35 + a34 * b45 + a35 * b55 + a36 * b65,
a30 * b06 + a31 * b16 + a32 * b26 + a33 * b36 + a34 * b46 + a35 * b56 + a36 * b66,
a40 * b00 + a41 * b10 + a42 * b20 + a43 * b30 + a44 * b40 + a45 * b50 + a46 * b60,
a40 * b01 + a41 * b11 + a42 * b21 + a43 * b31 + a44 * b41 + a45 * b51 + a46 * b61,
a40 * b02 + a41 * b12 + a42 * b22 + a43 * b32 + a44 * b42 + a45 * b52 + a46 * b62,
a40 * b03 + a41 * b13 + a42 * b23 + a43 * b33 + a44 * b43 + a45 * b53 + a46 * b63,
a40 * b04 + a41 * b14 + a42 * b24 + a43 * b34 + a44 * b44 + a45 * b54 + a46 * b64,
a40 * b05 + a41 * b15 + a42 * b25 + a43 * b35 + a44 * b45 + a45 * b55 + a46 * b65,
a40 * b06 + a41 * b16 + a42 * b26 + a43 * b36 + a44 * b46 + a45 * b56 + a46 * b66,
a50 * b00 + a51 * b10 + a52 * b20 + a53 * b30 + a54 * b40 + a55 * b50 + a56 * b60,
a50 * b01 + a51 * b11 + a52 * b21 + a53 * b31 + a54 * b41 + a55 * b51 + a56 * b61,
a50 * b02 + a51 * b12 + a52 * b22 + a53 * b32 + a54 * b42 + a55 * b52 + a56 * b62,
a50 * b03 + a51 * b13 + a52 * b23 + a53 * b33 + a54 * b43 + a55 * b53 + a56 * b63,
a50 * b04 + a51 * b14 + a52 * b24 + a53 * b34 + a54 * b44 + a55 * b54 + a56 * b64,
a50 * b05 + a51 * b15 + a52 * b25 + a53 * b35 + a54 * b45 + a55 * b55 + a56 * b65,
a50 * b06 + a51 * b16 + a52 * b26 + a53 * b36 + a54 * b46 + a55 * b56 + a56 * b66,
a60 * b00 + a61 * b10 + a62 * b20 + a63 * b30 + a64 * b40 + a65 * b50 + a66 * b60,
a60 * b01 + a61 * b11 + a62 * b21 + a63 * b31 + a64 * b41 + a65 * b51 + a66 * b61,
a60 * b02 + a61 * b12 + a62 * b22 + a63 * b32 + a64 * b42 + a65 * b52 + a66 * b62,
a60 * b03 + a61 * b13 + a62 * b23 + a63 * b33 + a64 * b43 + a65 * b53 + a66 * b63,
a60 * b04 + a61 * b14 + a62 * b24 + a63 * b34 + a64 * b44 + a65 * b54 + a66 * b64,
a60 * b05 + a61 * b15 + a62 * b25 + a63 * b35 + a64 * b45 + a65 * b55 + a66 * b65,
a60 * b06 + a61 * b16 + a62 * b26 + a63 * b36 + a64 * b46 + a65 * b56 + a66 * b66}
end
def mult_77_77t_plus_q77diag(a77, b77, q77) do
{
a00,
a01,
a02,
a03,
a04,
a05,
a06,
a10,
a11,
a12,
a13,
a14,
a15,
a16,
a20,
a21,
a22,
a23,
a24,
a25,
a26,
a30,
a31,
a32,
a33,
a34,
a35,
a36,
a40,
a41,
a42,
a43,
a44,
a45,
a46,
a50,
a51,
a52,
a53,
a54,
a55,
a56,
a60,
a61,
a62,
a63,
a64,
a65,
a66
} = a77
{
b00,
b01,
b02,
b03,
b04,
b05,
b06,
b10,
b11,
b12,
b13,
b14,
b15,
b16,
b20,
b21,
b22,
b23,
b24,
b25,
b26,
b30,
b31,
b32,
b33,
b34,
b35,
b36,
b40,
b41,
b42,
b43,
b44,
b45,
b46,
b50,
b51,
b52,
b53,
b54,
b55,
b56,
b60,
b61,
b62,
b63,
b64,
b65,
b66
} = b77
{q00, q11, q22, q33, q44, q55, q66} = q77
{q00 + a00 * b00 + a01 * b01 + a02 * b02 + a03 * b03 + a04 * b04 + a05 * b05 + a06 * b06,
a00 * b10 + a01 * b11 + a02 * b12 + a03 * b13 + a04 * b14 + a05 * b15 + a06 * b16,
a00 * b20 + a01 * b21 + a02 * b22 + a03 * b23 + a04 * b24 + a05 * b25 + a06 * b26,
a00 * b30 + a01 * b31 + a02 * b32 + a03 * b33 + a04 * b34 + a05 * b35 + a06 * b36,
a00 * b40 + a01 * b41 + a02 * b42 + a03 * b43 + a04 * b44 + a05 * b45 + a06 * b46,
a00 * b50 + a01 * b51 + a02 * b52 + a03 * b53 + a04 * b54 + a05 * b55 + a06 * b56,
a00 * b60 + a01 * b61 + a02 * b62 + a03 * b63 + a04 * b64 + a05 * b65 + a06 * b66,
a10 * b00 + a11 * b01 + a12 * b02 + a13 * b03 + a14 * b04 + a15 * b05 + a16 * b06,
q11 + a10 * b10 + a11 * b11 + a12 * b12 + a13 * b13 + a14 * b14 + a15 * b15 + a16 * b16,
a10 * b20 + a11 * b21 + a12 * b22 + a13 * b23 + a14 * b24 + a15 * b25 + a16 * b26,
a10 * b30 + a11 * b31 + a12 * b32 + a13 * b33 + a14 * b34 + a15 * b35 + a16 * b36,
a10 * b40 + a11 * b41 + a12 * b42 + a13 * b43 + a14 * b44 + a15 * b45 + a16 * b46,
a10 * b50 + a11 * b51 + a12 * b52 + a13 * b53 + a14 * b54 + a15 * b55 + a16 * b56,
a10 * b60 + a11 * b61 + a12 * b62 + a13 * b63 + a14 * b64 + a15 * b65 + a16 * b66,
a20 * b00 + a21 * b01 + a22 * b02 + a23 * b03 + a24 * b04 + a25 * b05 + a26 * b06,
a20 * b10 + a21 * b11 + a22 * b12 + a23 * b13 + a24 * b14 + a25 * b15 + a26 * b16,
q22 + a20 * b20 + a21 * b21 + a22 * b22 + a23 * b23 + a24 * b24 + a25 * b25 + a26 * b26,
a20 * b30 + a21 * b31 + a22 * b32 + a23 * b33 + a24 * b34 + a25 * b35 + a26 * b36,
a20 * b40 + a21 * b41 + a22 * b42 + a23 * b43 + a24 * b44 + a25 * b45 + a26 * b46,
a20 * b50 + a21 * b51 + a22 * b52 + a23 * b53 + a24 * b54 + a25 * b55 + a26 * b56,
a20 * b60 + a21 * b61 + a22 * b62 + a23 * b63 + a24 * b64 + a25 * b65 + a26 * b66,
a30 * b00 + a31 * b01 + a32 * b02 + a33 * b03 + a34 * b04 + a35 * b05 + a36 * b06,
a30 * b10 + a31 * b11 + a32 * b12 + a33 * b13 + a34 * b14 + a35 * b15 + a36 * b16,
a30 * b20 + a31 * b21 + a32 * b22 + a33 * b23 + a34 * b24 + a35 * b25 + a36 * b26,
q33 + a30 * b30 + a31 * b31 + a32 * b32 + a33 * b33 + a34 * b34 + a35 * b35 + a36 * b36,
a30 * b40 + a31 * b41 + a32 * b42 + a33 * b43 + a34 * b44 + a35 * b45 + a36 * b46,
a30 * b50 + a31 * b51 + a32 * b52 + a33 * b53 + a34 * b54 + a35 * b55 + a36 * b56,
a30 * b60 + a31 * b61 + a32 * b62 + a33 * b63 + a34 * b64 + a35 * b65 + a36 * b66,
a40 * b00 + a41 * b01 + a42 * b02 + a43 * b03 + a44 * b04 + a45 * b05 + a46 * b06,
a40 * b10 + a41 * b11 + a42 * b12 + a43 * b13 + a44 * b14 + a45 * b15 + a46 * b16,
a40 * b20 + a41 * b21 + a42 * b22 + a43 * b23 + a44 * b24 + a45 * b25 + a46 * b26,
a40 * b30 + a41 * b31 + a42 * b32 + a43 * b33 + a44 * b34 + a45 * b35 + a46 * b36,
q44 + a40 * b40 + a41 * b41 + a42 * b42 + a43 * b43 + a44 * b44 + a45 * b45 + a46 * b46,
a40 * b50 + a41 * b51 + a42 * b52 + a43 * b53 + a44 * b54 + a45 * b55 + a46 * b56,
a40 * b60 + a41 * b61 + a42 * b62 + a43 * b63 + a44 * b64 + a45 * b65 + a46 * b66,
a50 * b00 + a51 * b01 + a52 * b02 + a53 * b03 + a54 * b04 + a55 * b05 + a56 * b06,
a50 * b10 + a51 * b11 + a52 * b12 + a53 * b13 + a54 * b14 + a55 * b15 + a56 * b16,
a50 * b20 + a51 * b21 + a52 * b22 + a53 * b23 + a54 * b24 + a55 * b25 + a56 * b26,
a50 * b30 + a51 * b31 + a52 * b32 + a53 * b33 + a54 * b34 + a55 * b35 + a56 * b36,
a50 * b40 + a51 * b41 + a52 * b42 + a53 * b43 + a54 * b44 + a55 * b45 + a56 * b46,
q55 + a50 * b50 + a51 * b51 + a52 * b52 + a53 * b53 + a54 * b54 + a55 * b55 + a56 * b56,
a50 * b60 + a51 * b61 + a52 * b62 + a53 * b63 + a54 * b64 + a55 * b65 + a56 * b66,
a60 * b00 + a61 * b01 + a62 * b02 + a63 * b03 + a64 * b04 + a65 * b05 + a66 * b06,
a60 * b10 + a61 * b11 + a62 * b12 + a63 * b13 + a64 * b14 + a65 * b15 + a66 * b16,
a60 * b20 + a61 * b21 + a62 * b22 + a63 * b23 + a64 * b24 + a65 * b25 + a66 * b26,
a60 * b30 + a61 * b31 + a62 * b32 + a63 * b33 + a64 * b34 + a65 * b35 + a66 * b36,
a60 * b40 + a61 * b41 + a62 * b42 + a63 * b43 + a64 * b44 + a65 * b45 + a66 * b46,
a60 * b50 + a61 * b51 + a62 * b52 + a63 * b53 + a64 * b54 + a65 * b55 + a66 * b56,
q66 + a60 * b60 + a61 * b61 + a62 * b62 + a63 * b63 + a64 * b64 + a65 * b65 + a66 * b66}
end
@spec to_2d_list(list(), integer(), integer()) :: list()
def to_2d_list(matrix, rows, columns) do
{_, mat_2d_list} =
Enum.reduce(1..rows, {matrix, []}, fn _row_num, {matrix_rem, acc} ->
{row, matrix_rem} = Enum.split(matrix_rem, columns)
{matrix_rem, acc ++ [row]}
end)
Logger.debug(inspect(mat_2d_list))
mat_2d_list
end
@spec generate_ekf_cov(list()) :: tuple()
def generate_ekf_cov(config) do
init_std_devs =
Keyword.fetch!(config, :init_std_devs)
|> Enum.with_index()
ekf_cov_list =
Enum.reduce(init_std_devs, [], fn {std_dev, index}, acc ->
row =
[0, 0, 0, 0, 0, 0, 0]
|> List.replace_at(index, std_dev * std_dev)
acc ++ row
end)
List.to_tuple(ekf_cov_list)
end
@spec generate_r_gps(list()) :: tuple()
def generate_r_gps(config) do
keys = [
:gpspos_xy_std,
:gpspos_xy_std,
:gpspos_z_std,
:gpsvel_xy_std,
:gpsvel_xy_std,
:gpsvel_z_std
]
Enum.map(keys, fn key ->
value = Keyword.fetch!(config, key)
value * value
end)
|> List.to_tuple()
end
@spec generate_r_heading(list()) :: tuple()
def generate_r_heading(config) do
value = Keyword.fetch!(config, :gpsyaw_std)
{value * value}
end
@spec generate_q(list()) :: tuple()
def generate_q(config) do
keys = [
:qpos_xy_std,
:qpos_xy_std,
:qpos_z_std,
:qvel_xy_std,
:qvel_xy_std,
:qvel_z_std,
:qyaw_std
]
expected_imu_dt_s = Keyword.fetch!(config, :expected_imu_dt_s)
Enum.map(keys, fn key ->
value = Keyword.fetch!(config, key)
value * value * expected_imu_dt_s
end)
|> List.to_tuple()
end
@spec position_rrm(struct()) :: struct()
def position_rrm(state) do
if is_nil(state.origin) do
nil
else
{latitude_rad, longitude_rad, position_down_m, _, _, _, _} = state.ekf_state
ViaUtils.Location.location_from_point_with_dx_dy(state.origin, latitude_rad, longitude_rad)
|> Map.put(:altitude_m, -position_down_m)
end
end
@spec velocity_mps(struct()) :: map()
def velocity_mps(state) do
{_, _, _, v_north_mps, v_east_mps, v_down_mps} = state.ekf_state
%{north_mps: v_north_mps, east_mps: v_east_mps, down_mps: v_down_mps}
end
@spec position_rrm_velocity_mps(struct()) :: tuple()
def position_rrm_velocity_mps(state) do
{latitude_rad, longitude_rad, position_down_m, v_north_mps, v_east_mps, v_down_mps, _} =
state.ekf_state
position_rrm =
if is_nil(state.origin) do
nil
else
ViaUtils.Location.location_from_point_with_dx_dy(
state.origin,
latitude_rad,
longitude_rad
)
|> Map.put(:altitude_m, -position_down_m)
end
velocity_mps = %{north_mps: v_north_mps, east_mps: v_east_mps, down_mps: v_down_mps}
{position_rrm, velocity_mps}
end
end