class ExtendedKalmanFilter {
constructor(state, covariance) {
this.state = state
this.covariance = covariance
}
predict(dt) {
const stateTransitionMatrix = this.jacobianF(this.state, dt)
const jacobianMatrix = this.jacobianF(this.state, dt)
this.state = this.stateTransition(this.state, dt)
const covariance = this.covariance
this.covariance = MatrixUtils.sum(
MatrixUtils.multiply(
stateTransitionMatrix,
MatrixUtils.multiply(
covariance,
MatrixUtils.transpose(stateTransitionMatrix)
)
),
MatrixUtils.multiply(
jacobianMatrix,
MatrixUtils.multiply(covariance, MatrixUtils.transpose(jacobianMatrix))
)
)
const q = [
[10, 0, 0],
[0, 10, 0],
[0, 0, 10]
]
this.covariance = MatrixUtils.sum(this.covariance, q)
}
// Update the state and covariance given a new observation
update(observation, measurementNoise) {
// Calculate the observation matrix and Jacobian
const observationMatrix = this.jacobianH(this.state)
// Calculate the innovation and innovation covariance
const innovation = observation - this.state.price
const innovationCovariance = MatrixUtils.sum(
MatrixUtils.multiply(
observationMatrix,
MatrixUtils.multiply(
this.covariance,
MatrixUtils.transpose(observationMatrix)
)
),
measurementNoise
)
// Calculate the Kalman gain
const kalmanGain = MatrixUtils.multiply(
this.covariance,
MatrixUtils.transpose(observationMatrix)
).map(row => row.map(value => value / innovationCovariance[0][0]))
// Update the state and covariance using the Kalman gain and innovation
const deltaState = MatrixUtils.multiply(kalmanGain, [[innovation]])
this.state.price += deltaState[0][0]
this.state.velocity += deltaState[1][0]
this.state.acceleration += deltaState[2][0]
this.covariance = MatrixUtils.subtract(
this.covariance,
MatrixUtils.multiply(
kalmanGain,
MatrixUtils.multiply(observationMatrix, this.covariance)
)
)
}
// Define the state transition function f(x)
stateTransition(state, dt) {
const price =
state.price + state.velocity * dt + 0.5 * state.acceleration * dt * dt
const velocity = state.velocity + state.acceleration * dt
const acceleration = state.acceleration
return { price, velocity, acceleration }
}
// Define the Jacobian of the state transition function f(x)
jacobianF(state, dt) {
return [
[1, dt, 0.5 * dt * dt],
[0, 1, dt],
[0, 0, 1]
]
}
// Define the Jacobian of the observation function h(x)
jacobianH(state) {
return [[1, 0, 0]]
}
}