ignore my last reply; I have some updates and I only need help on three final things; I’ll send u the new info soon
Thank you for your support! I almost got things working, but I ran into a few problems I can’t seem to fix: (1) the scooter just floats in the air after getting off a slope. (2) it doesn’t always turn to the projected move vector; if projected move vector is backwards relative to scooter, it turns anti-parallel to the projected move vector and just moves backward when I actually want it to turn all the way around to face the projected move vector and move forward (3) the linear velocity constraint takes a while before it actually has an effect and moves the scooter
Video:
The floating character is intentional; just a lazy way to essentially get the same setup as in the actual game where the character is on the scooter (im using basic test game)
Setup:
- Workspace
- Model named “Scooter”
- Part named “ScooterPhysicsBox”
- Attachment
- AlignOrientation constraint named “ScooterAlignOrientation”
- LinearVelocity constraint named “ScooterLinearVelocity”
- Part named “ScooterPhysicsBox”
- Model named “Scooter”
- StarterPlayer
- StarterPlayerScripts
- ScooterSetup.lua ← local script
- Math.lua ← module script
- ScooterSetup.lua ← local script
- StarterPlayerScripts
Properties:
The scooter physics box: (size: <2, 4, 6>)
The align orientation constraint (responsiveness: 50; mode: OneAttachment; axes: AllAxes; max torque: 1e10; attachment0: scooter physics box attachment)
The linear velocity constraint (mode: Vector, force limits enabled: true but I switched to false in video; relative to: attachment0; max force: 1e10; attachment0: scooter physics box attachment)
Scripts:
ScooterSetup.lua
--[[ Services ]]
local Players: Players = game:GetService("Players")
local RunService: RunService = game:GetService("RunService")
--[[ Game Objects ]]
-- Player
local player: Player = Players.LocalPlayer :: Player
local playerScripts: PlayerScripts = player:WaitForChild("PlayerScripts") :: PlayerScripts
local playerModuleScript: ModuleScript = playerScripts:WaitForChild("PlayerModule") :: ModuleScript
local masterControlScript: ModuleScript = playerModuleScript:WaitForChild("ControlModule") :: ModuleScript
local character: Model = player.Character or player.CharacterAdded:Wait()
local humanoid: Humanoid = character:WaitForChild("Humanoid") :: Humanoid
local primaryPart: Part = character:WaitForChild("HumanoidRootPart") :: Part
local leftFoot: BasePart = character:WaitForChild("LeftFoot", 5) or character:WaitForChild("Left Leg")
local rightFoot: BasePart = character:WaitForChild("RightFoot", 5) or character:WaitForChild("Right Leg")
-- Scooter
local scooter: Model = workspace:WaitForChild("Scooter") :: Model
local scooterPhysicsBox: Part = scooter:WaitForChild("ScooterPhysicsBox") :: Part
local linearVelocity: LinearVelocity = scooter:WaitForChild("ScooterLinearVelocity") :: LinearVelocity
local alignOrientation: AlignOrientation = scooter:WaitForChild("ScooterAlignOrientation") :: AlignOrientation
-- Camera
local camera: Camera = workspace.CurrentCamera
--[[ Dependencies ]]
-- Math
local Math = require(script.Math)
local Vector = Math.Vector
local KalmanFilter = Math.KalmanFilter
-- Controls
local MasterControl = require(masterControlScript)
--[[ Configuration ]]
local maxLinearSpeed = 55
local angularSpeed = 5
local linearAcceleration = maxLinearSpeed / 2.3
local floorOffset = scooterPhysicsBox.Size.Y * 0.5125 -- desired distance between centroid of scooter and rest surface
local floorOffsetErrorThreshold = 1e-1 -- allowed error from floor offset (was FLOOR_EPSILON)
local updateFrequency = 1 / 60 -- minimum frequency to update state so as to avoid overhead
local smoothFrameTimeDeltaLinearInterpolationAlpha = 0.2 -- lerp alpha for smooth delta
--[[ Constants ]]
-- Relative Vectors
local worldUpVector = Vector3.yAxis
local worldHorizontalVector = Vector3.xAxis + Vector3.zAxis
local gravityBaseVector = -worldUpVector
-- Mechanics
local gravitationalAcceleration = gravityBaseVector * workspace.Gravity
local maxSlopeAngle = math.rad(89) -- arbitrary maximum slope angle
local maxSlopeVector = worldUpVector * math.sin(maxSlopeAngle)
local scooterWidth = scooterPhysicsBox.Size.X
local scooterHeight = scooterPhysicsBox.Size.Y
local scooterDepth = scooterPhysicsBox.Size.Z
-- Raycasts
local internalRaycast = workspace.Raycast
local raycastDirection = -1 * (floorOffset * 2 + scooterHeight * 2)
local raycastParams = RaycastParams.new()
raycastParams.FilterType = Enum.RaycastFilterType.Exclude
raycastParams.FilterDescendantsInstances = {scooter, character}
raycastParams.RespectCanCollide = true
--[[ Variables ]]
-- Kalman Filter
local transform = scooterPhysicsBox.CFrame
local normalFilter = KalmanFilter.new(Vector.fromVector3(transform.UpVector), 1e-4)
-- Framing
local lastFrameTime: number = nil
local lastFrameTimeDelta: number = nil
local currentTime: number = os.clock()
local currentFrameTime = 0
-- Movement
local movement = {
accelerationDirection = 0, -- 1 = forward; -1 = backward; 0 = neither
turnDirection = 0 -- 1 = counter-clockwise; -1 = clockwise; 0 = neither
}
-- weld character to scooter (temporary for testing purposes and a basic setup)
character.PrimaryPart.CFrame = scooterPhysicsBox.CFrame + scooterPhysicsBox.CFrame.UpVector * 10
local weld = Instance.new("WeldConstraint")
weld.Part0 = scooterPhysicsBox
weld.Part1 = character.HumanoidRootPart
weld.Parent = scooterPhysicsBox
--[[ Functions ]]
local function computeSmoothDelta(inputCurrentTime: number)
inputCurrentTime = if inputCurrentTime then inputCurrentTime else os.clock()
if not lastFrameTime or not lastFrameTimeDelta then
lastFrameTime = inputCurrentTime
lastFrameTimeDelta = updateFrequency
return lastFrameTimeDelta
end
local delta = inputCurrentTime - lastFrameTime
lastFrameTimeDelta = Math.linearInterpolation(lastFrameTimeDelta, delta, smoothFrameTimeDeltaLinearInterpolationAlpha)
lastFrameTime = inputCurrentTime
return math.floor(lastFrameTimeDelta * 1000 + 0.5) / 1000
end
local function raycast(origin: Vector3, direction: Vector3, parameters: RaycastParams): (Instance, Vector3, Vector3, Enum.Material, number)
local result = internalRaycast(workspace, origin, direction, parameters)
local intersectedInstance: Instance
local interectionPosition: Vector3
local intersectionSurfaceNormal: Vector3
local intersectedSurfaceMaterial: Enum.Material
local distanceBetweenOriginAndIntersectionPoint: number
if result == nil then
intersectedInstance = nil
interectionPosition = origin + direction
intersectionSurfaceNormal = worldUpVector
distanceBetweenOriginAndIntersectionPoint = direction.Magnitude
else
intersectedInstance = result.Instance
interectionPosition = result.Position
intersectionSurfaceNormal = result.Normal
intersectedSurfaceMaterial = result.Material
distanceBetweenOriginAndIntersectionPoint = result.Distance
end
return intersectedInstance, interectionPosition, intersectionSurfaceNormal, intersectedSurfaceMaterial, distanceBetweenOriginAndIntersectionPoint
end
local function computeFaceNormal(pointA: Vector3, pointB: Vector3, pointC: Vector3, vertexNormal: Vector3?): Vector3
local p0 = pointB - pointA
local p1 = pointC - pointA
local normal = p0:Cross(p1)
local d = normal.X * normal.X + normal.Y * normal.Y + normal.Z * normal.Z
if d > 1 then
normal = normal.Unit
end
if vertexNormal ~= nil then
d = normal:Dot(vertexNormal)
return if d < 0 then -normal else normal
end
return normal
end
local function updateMovement()
local rawMoveVector: Vector3 = MasterControl:GetMoveVector()
if rawMoveVector.Magnitude == 0 then
movement.accelerationDirection = 0
movement.turnDirection = 0
return
end
local cameraLookVector = camera.CFrame.LookVector
local cameraRightVector = camera.CFrame.RightVector
local scooterLookVector2 = Vector2.new(scooterPhysicsBox.CFrame.LookVector.X, scooterPhysicsBox.CFrame.LookVector.Z).Unit
local projectedMoveVector = rawMoveVector.X * cameraRightVector + -rawMoveVector.Z * cameraLookVector
local projectedMoveVector2 = Vector2.new(projectedMoveVector.X, projectedMoveVector.Z).Unit
local dotProduct = projectedMoveVector2:Dot(scooterLookVector2)
movement.accelerationDirection = math.sign(dotProduct)
if math.abs(dotProduct) >= 0.99 then
-- do not turn if difference between projected move vector and scooter look vector is insignificant
movement.turnDirection = 0
else
-- determine if clockwise/counter-clockwise: https://gamedev.stackexchange.com/questions/45412/understanding-math-used-to-determine-if-vector-is-clockwise-counterclockwise-f
movement.turnDirection = if projectedMoveVector2.Y * scooterLookVector2.X > projectedMoveVector2.X * scooterLookVector2.Y then -1 else 1
end
end
local function computeVelocity(deltaTime: number): Vector3
if movement.accelerationDirection ~= -math.sign(linearVelocity.VectorVelocity.Z) and linearVelocity.VectorVelocity.Magnitude ~= 0 then
local newVelocity = linearVelocity.VectorVelocity + Vector3.new(0, 0, -math.sign(linearVelocity.VectorVelocity.Z) * linearAcceleration * deltaTime)
if math.sign(newVelocity.Z) == -math.sign(linearVelocity.VectorVelocity.Z) then newVelocity = Vector3.zero end
return newVelocity
else
local newVelocity = linearVelocity.VectorVelocity + Vector3.new(0, 0, -movement.accelerationDirection * linearAcceleration * deltaTime)
if newVelocity.Magnitude > maxLinearSpeed then newVelocity = newVelocity.Unit * maxLinearSpeed end
return newVelocity
end
end
local function onStepped()
currentTime = os.clock()
local smoothDelta = computeSmoothDelta(currentTime)
currentFrameTime += smoothDelta
if currentFrameTime < updateFrequency then return end
local deltaTime = currentFrameTime
currentFrameTime = math.fmod(currentFrameTime, updateFrequency)
updateMovement()
local acceleration = movement.accelerationDirection
local turn = movement.turnDirection
local linearVelocitySign = if acceleration >= 0 then 1 else -1
turn *= angularSpeed * smoothDelta * linearVelocitySign
acceleration *= maxLinearSpeed * smoothDelta * 10
local turnAngle = CFrame.Angles(0, turn, 0)
transform = scooterPhysicsBox.CFrame
-- Detect surface normal by casting 3 rays downward around scoooter
local translation = transform.Position
local upVector = transform.UpVector
local lookVector = transform.LookVector
local rightVector = transform.RightVector
local rayOrigin1 = translation - rightVector * scooterWidth + upVector * scooterHeight * 2 - lookVector * scooterDepth
local rayOrigin2 = translation + rightVector * scooterWidth + upVector * scooterHeight * 2 - lookVector * scooterDepth
local rayOrigin3 = translation + lookVector * scooterDepth + upVector * scooterHeight * 2
local down = transform:VectorToWorldSpace(-gravityBaseVector) * raycastDirection
local instersectedInstance1, intersectionPoint1 = raycast(rayOrigin1, down, raycastParams)
local instersectedInstance2, intersectionPoint2 = raycast(rayOrigin2, down, raycastParams)
local instersectedInstance3, intersectionPoint3 = raycast(rayOrigin3, down, raycastParams)
-- Apply gravity if new intersection + height is greater than desired floor offset
local floorHeight = math.max(intersectionPoint1.Y, intersectionPoint2.Y, intersectionPoint3.Y)
local currentHeight = translation.Y
local displacement = currentHeight - floorHeight
if displacement > floorOffset + floorOffsetErrorThreshold then
floorHeight = currentHeight + gravitationalAcceleration.Y * smoothDelta
else
floorHeight += floorOffset
end
-- derive normal vector
local normal: Vector3
if intersectionPoint1 ~= nil and intersectionPoint2 ~= nil and intersectionPoint3 ~= nil then
-- since all intersections points exist, compute face normal
normal = computeFaceNormal(intersectionPoint1, intersectionPoint2, intersectionPoint3)
else
-- revert to world up vector
normal = worldUpVector
end
-- compute nomal in relation to current and maximum slope angle
local slope = math.acos(normal.Y)
if slope > maxSlopeAngle then
normal = worldUpVector * math.sin(slope - maxSlopeAngle) + maxSlopeVector / math.sin(slope)
end
-- filter normal vector
normalFilter = normalFilter.update(Vector.fromVector3(normal))
normal = normalFilter.state.toVector3()
-- apply results to transform
translation = translation * worldHorizontalVector + worldUpVector * floorHeight + lookVector * acceleration
lookVector = turnAngle:VectorToWorldSpace(transform.LookVector)
rightVector = turnAngle:VectorToWorldSpace(lookVector:Cross(normal))
transform = CFrame.fromMatrix(
translation,
rightVector,
normal,
-lookVector
)
linearVelocity.VectorVelocity = computeVelocity(deltaTime)
alignOrientation.CFrame = transform
end
RunService.Stepped:Connect(onStepped)
Math.lua
local Math = {
Sequence = {},
Tensor = {},
Vector = {},
KalmanFilter = {}
}
--[[ Sequence ]]
export type Sequence<T> = {
elements: {T},
size: number,
forEach: ((index: number, element: T, cancel: () -> ()) -> ()) -> (),
map: <M>((index: number, element: T) -> M) -> Sequence<M>,
contains: (element: T) -> boolean
}
function Math.Sequence.new<T>(...: T): Sequence<T>
local self: Sequence<T> = {}
self.size = select("#", ...)
self.elements = {...}
self.forEach = function(closure: (number, T, () -> ()) -> ())
if self.size == 0 then return end
local shouldBreak = false
for i = 1, self.size do
closure(i, self.elements[i], function() shouldBreak = true end)
if shouldBreak then break end
end
end
self.map = function<M>(transform: (number, T) -> M): Sequence<M>
local mappedElements: {M} = {}
self.forEach(function(index: number, element: T)
table.insert(mappedElements, transform(index, element))
end)
return Math.Sequence.fromTable(mappedElements)
end
self.contains = function(element: T): boolean
return table.find(self.elements, element) ~= nil
end
return self
end
function Math.Sequence.fromTable<T>(tab: {T}): Sequence<T>
return Math.Sequence.new(unpack(tab))
end
--[[ Tensor ]]
export type Tensor = {
data: Sequence<Sequence<number>>,
add: (otherTensor: Tensor) -> Tensor,
subtract: (otherTensor: Tensor) -> Tensor,
scalarMultiply: (multiplier: number) -> Tensor,
scalarDivide: (dividend: number) -> Tensor
}
function Math.Tensor.new(...: Sequence<number>): Tensor
local self: Tensor = {}
self.data = Math.Sequence.new(...)
self.add = function(otherTensor: Tensor): Tensor
assert(otherTensor.data.size == self.data.size, "Tensors must be of the same dimensions to add them")
return Math.Tensor.fromTable(self.data.map(function(rowIndex: number, row: Sequence<number>)
assert(row.size == otherTensor.data.elements[rowIndex].size, "Tensors must be of the same dimensions to add them")
return row.map(function(columnIndex: number, element: number)
return element + otherTensor.data.elements[rowIndex][columnIndex]
end)
end))
end
self.subtract = function(otherTensor: Tensor): Tensor
assert(otherTensor.data.size == self.data.size, "Tensors must be of the same dimensions to subtract them")
return Math.Tensor.fromTable(self.data.map(function(rowIndex: number, row: Sequence<number>)
assert(row.size == otherTensor.data.elements[rowIndex].size, "Tensors must be of the same dimensions to subtract them")
return row.map(function(columnIndex: number, element: number)
return element - otherTensor.data.elements[rowIndex][columnIndex]
end)
end))
end
self.scalarMultiply = function(multiplier: number): Tensor
assert(multiplier == multiplier, "Cannot scalar multiply Tensor by NaN")
return Math.Tensor.fromTable(self.data.map(function(rowIndex: number, row: Sequence<number>)
return row.map(function(columnIndex: number, element: number)
return element * multiplier
end)
end))
end
self.scalarDivide = function(dividend: number): Tensor
assert(dividend ~= 0, "Cannot scalar divide Tensor by 0")
assert(dividend == dividend, "Cannot scalar divide Tensor by NaN")
return Math.Tensor.fromTable(self.data.map(function(rowIndex: number, row: Sequence<number>)
return row.map(function(columnIndex: number, element: number)
return element / dividend
end)
end))
end
return self
end
function Math.Tensor.fromTable<T>(tab: {T}): Tensor
return Math.Tensor.new(Math.Sequence.fromTable(tab))
end
function Math.Tensor.fromSequence<T>(sequence: Sequence<T>): Tensor
return Math.Tensor.fromTable(sequence.elements)
end
--[[ Vector ]]
export type Vector = {
dimensions: number,
components: Sequence<number>,
squareMagnitude: () -> number,
magnitude: () -> number,
add: (otherVector: Vector) -> Vector,
subtract: (otherVector: Vector) -> Vector,
multiply: (otherVector: Vector) -> Vector,
divide: (otherVector: Vector) -> Vector,
scalarMultiply: (multiplier: number) -> Vector,
scalarDivide: (dividend: number) -> Vector,
dotProduct: (otherVector: Vector) -> number,
tensorProduct: (otherVector: Vector) -> Tensor,
exteriorProduct: (otherVector: Vector) -> Vector,
geometricProduct: (otherVector: Vector) -> Vector,
crossProduct: (Vector) -> Vector | number,
normalized: () -> Vector,
toVector2: () -> Vector2,
toVector3: () -> Vector3,
toString: () -> string
}
function Math.Vector.new(...: number): Vector
local numComponents = select("#", ...)
assert(numComponents >= 2, "Vector must have at least two components")
local self: Vector = {}
self.dimensions = numComponents
self.components = Math.Sequence.new(...)
self.squareMagnitude = function(): number
local sum = 0
self.components.forEach(function(_: number, component: number, _: () -> ())
sum += component * component
end)
return sum
end
self.magnitude = function(): number
return math.sqrt(self.squareMagnitude())
end
self.add = function(otherVector: Vector): Vector
assert(otherVector.dimensions == self.dimensions, "Other vector must have the same dimensions as this vector to add them")
return Math.Vector.fromSequence(self.components.map(function(index: number, component: number)
return component + otherVector.components.elements[index]
end))
end
self.subtract = function(otherVector: Vector): Vector
assert(otherVector.dimensions == self.dimensions, "Other vector must have the same dimensions as this vector to subtract them")
return Math.Vector.fromSequence(self.components.map(function(index: number, component: number)
return component - otherVector.components.elements[index]
end))
end
self.multiply = function(otherVector: Vector): Vector
assert(otherVector.dimensions == self.dimensions, "Other vector must have the same dimensions as this vector to multiply them")
return Math.Vector.fromSequence(self.components.map(function(index: number, component: number)
return component * otherVector.components.elements[index]
end))
end
self.divide = function(otherVector: Vector): Vector
assert(otherVector.dimensions == self.dimensions, "Other vector must have the same dimensions as this vector to divide them")
return Math.Vector.fromSequence(self.components.map(function(index: number, component: number)
assert(otherVector.components.elements[index] ~= 0, "Cannot divide by a Vector with a zero component")
return component / otherVector.components.elements[index]
end))
end
self.scalarMultiply = function(multiplier: number): Vector
assert(multiplier == multiplier, "Cannot scalar multiply Vector by NaN")
return Math.Vector.fromSequence(self.components.map(function(_: number, component: number)
return component * multiplier
end))
end
self.scalarDivide = function(dividend: number): Vector3
assert(dividend ~= 0, "Cannot scalar divide Vector by 0")
assert(dividend == dividend, "Cannot scalar divide Vector by NaN")
return Math.Vector.fromSequence(self.components.map(function(_: number, component: number)
return component / dividend
end))
end
self.dotProduct = function(otherVector: Vector): number
assert(otherVector.dimensions == self.dimensions, "Other vector must have the same dimensions as this vector to calculate the dot product")
local sum = 0
self.components.forEach(function(index: number, component: number, _: () -> ())
sum += component * otherVector.components.elements[index]
end)
return sum
end
self.tensorProduct = function(otherVector: Vector): Tensor
return Math.Tensor.fromSequence(self.components.map(function(_: number, component: number)
return otherVector.components.map(function(_: number, otherComponent: number)
return component * otherComponent
end)
end))
end
self.exteriorProduct = function(otherVector: Vector): Tensor
assert(otherVector.dimensions == self.dimensions, "Other vector must have the same dimensions as this vector to calculate the exterior product")
return self.tensorProduct(otherVector).subtract(otherVector.tensorProduct(self)).scalarMultiply(0.5)
end
self.geometricProduct = function(otherVector: Vector): Vector
assert(otherVector.dimensions == self.dimensions, "Other vector must have the same dimensions as this vector to calculate the geometric product")
-- TODO: implement geometric product
error("Geometric product has not yet been implemented")
return self.dotProduct(otherVector) + self.exteriorProduct(otherVector)
end
self.crossProduct = function(otherVector: Vector): Vector | number
assert(otherVector.dimensions == self.dimensions, "Other vector must have the same dimensions as this vector to calculate the cross product")
if self.dimensions == 3 then
return Math.Vector.new(
self.components.elements[2] * otherVector.components.elements[3] - self.components.elements[3] * otherVector.components.elements[2],
self.components.elements[3] * otherVector.components.elements[1] - self.components.elements[1] * otherVector.components.elements[3],
self.components.elements[1] * otherVector.components.elements[2] - self.components.elements[2] * otherVector.components.elements[1]
)
elseif self.dimensions == 2 then
return self.components.elements[1] * otherVector.components.elements[2] - self.components.elements[2] * otherVector.components.elements[1]
else
-- TODO: implement cross product
error("Cross product of vectors of dimensions higher than 3 has not yet been implemented")
return self.exteriorProduct(otherVector).hodgeStar()
end
end
self.normalized = function(): Vector
local magnitude = self.magnitude()
return Math.Vector.fromSequence(self.components.map(function(_: number, component: number)
return if magnitude == 0 then 0 else component / magnitude
end))
end
self.toVector2 = function(): Vector2
assert(self.dimensions == 2, "Can only become a Vector2 if there are two dimensions")
return Vector2.new(self.components.elements[1], self.components.elements[2])
end
self.toVector3 = function(): Vector3
assert(self.dimensions == 3, "Can only become a Vector3 if there are three dimensions")
return Vector3.new(self.components.elements[1], self.components.elements[2], self.components.elements[3])
end
self.toString = function(): string
local function componentSeparator(index: number, component: number): string
if index == self.dimensions then return tostring(component) end
return tostring(component) .. ", "
end
local finalStr = ""
self.components.forEach(function(index: number, component: number, _: () -> ())
finalStr ..= tostring(component)
if index ~= self.dimensions then
finalStr ..= ", "
end
end)
return "<" .. finalStr .. ">"
end
return self
end
function Math.Vector.fromTable(tab: {number}): Vector
return Math.Vector.new(unpack(tab))
end
function Math.Vector.fromSequence(sequence: Sequence<number>): Vector
return Math.Vector.fromTable(sequence.elements)
end
function Math.Vector.fromVector3(vector: Vector3): Vector
return Math.Vector.new(vector.X, vector.Y, vector.Z)
end
--[[ Other API Functions ]]
function Math.linearInterpolation(start: number, target: number, alpha: number): number
return start + alpha * (target - start)
end
--[[ Kalman Filter ]]
export type KalmanFilter = {
state: number | Vector,
processNoiseCovariance: number,
measurementNoiseCovariance: number,
estimationErrorCovariance: number,
kalmanGain: number,
update: (measurement: number | Vector, processNoiseCovariance: number?, measurementNoiseCovariance: number?) -> KalmanFilter,
reset: (state: number) -> KalmanFilter
}
function Math.KalmanFilter.new(state: (number | Vector)?, processNoiseCovariance: number?, measurementNoiseCovariance: number?, estimationErrorCovariance: number?, kalmanGain: number?)
local self: KalmanFilter = {}
self.state = if state ~= nil and state == state then state else 0
self.processNoiseCovariance = if processNoiseCovariance ~= nil and processNoiseCovariance == processNoiseCovariance then processNoiseCovariance else 0.000001
self.measurementNoiseCovariance = if measurementNoiseCovariance ~= nil and measurementNoiseCovariance == measurementNoiseCovariance then measurementNoiseCovariance else 0.01
self.estimationErrorCovariance = if estimationErrorCovariance ~= nil and estimationErrorCovariance == estimationErrorCovariance then estimationErrorCovariance else 1
self.kalmanGain = if kalmanGain ~= nil and kalmanGain == kalmanGain then kalmanGain else 0
self.update = function(measurement: number | Vector, newProcessNoiseCovariance: number?, newMeasurementNoiseCovariance: number?): KalmanFilter
local processNoiseCovariance = if newProcessNoiseCovariance ~= nil and newProcessNoiseCovariance == newProcessNoiseCovariance then newProcessNoiseCovariance else self.processNoiseCovariance
local measurementNoiseCovariance = if newMeasurementNoiseCovariance ~= nil and newMeasurementNoiseCovariance == newMeasurementNoiseCovariance then newMeasurementNoiseCovariance else self.measurementNoiseCovariance
local estimationErrorCovariance = measurementNoiseCovariance * (self.estimationErrorCovariance + processNoiseCovariance) / (measurementNoiseCovariance + self.estimationErrorCovariance + processNoiseCovariance)
local kalmanGain = (estimationErrorCovariance + processNoiseCovariance) / (estimationErrorCovariance + processNoiseCovariance + measurementNoiseCovariance)
local state
if type(self.state) == "table" and type(measurement) == "table" then
state = self.state.add(measurement.subtract(self.state).scalarMultiply(kalmanGain))
else
state = self.state + (measurement - self.state) * kalmanGain
end
return Math.KalmanFilter.new(state, processNoiseCovariance, measurementNoiseCovariance, estimationErrorCovariance, kalmanGain)
end
self.reset = function(state: number): KalmanFilter
return Math.KalmanFilter.new(state, self.processNoiseCovariance, self.measurementNoiseCovariance, 1, 0)
end
return self
end
return Math
The most important functions that may help give a good start on where to look are the updateMovement for the incorrect turning bug and onStepped for the floating bug though idk how to fix the third bug