-
Notifications
You must be signed in to change notification settings - Fork 4
/
Copy pathkalman.jl
65 lines (53 loc) · 1.56 KB
/
kalman.jl
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
module kalman;
import StateSpace;
import Distributions;
export newModel,initialGuess,predict,update,extractMeanFromState,extractVarianceFromState;
sampleTimeDeltaTable = Dict{UTF8String,Float64}();
function initialGuess(observations::Array{Float64,1},varianceEstimate::Array{Float64,1})
state = [observations; [0.0,0.0,0.0]]
variance = [varianceEstimate; [0.0001,0.0001,0.0001]]
return Distributions.MvNormal(state,variance)
end
function fThunk(id)
function f(t)
dt = sampleTimeDeltaTable[id];
F = eye(6)
F[1,4] = dt
F[2,5] = dt
F[3,6] = dt
return F;
end
return f
end
function qThunk(id,accVar)
function q(t)
dt = sampleTimeDeltaTable[id];
G = [ dt^2/2; dt^2/2; dt^2/2; dt; dt; dt ]
Q = eye(6).*(G*G'*accVar);
return Q;
end
return q
end
function recordSampleTimeDelta(id,sampleTimeDelta)
sampleTimeDeltaTable[id] = sampleTimeDelta;
end
function newModel(id,accVar::Float64,obVar::Float64,dt0)
H = [ eye(3) zeros(3,3) ];
R = eye(3)*obVar;
recordSampleTimeDelta(id,dt0);
return StateSpace.LinearGaussianSSM(fThunk(id),(t)->zeros(Float64,6,1),qThunk(id,accVar),(t)->H,(t)->R);
end
function update(id,model,predictedState,observations,dt)
recordSampleTimeDelta(id,dt);
return StateSpace.update(model,predictedState,copy(observations));
end
function predict(id,model,state)
return StateSpace.predict(model,state)
end
function extractMeanFromState(state)
return Distributions.mean(state)
end
function extractVarianceFromState(state)
return Distributions.var(state)
end
end