Based upon Dan Simon's Mathematica Kalman filtering example calculation.

Kalman Filter Example Calculation

Delta t: t 0.5 Iterations: n 60

Acceleration noise:

a noise

0.5

Position Measurement noise:

PM noise

10

Transition Matrix:

1 t 0 1

Measurement Matrix:

(1 0 )

Initial State Vector:

x0

0 0

Initial State Estimate:

x hat

x0 t
4

t 2

Process noise covariance:

2 a noise .

4 t 2
3

Initial estimation covariance:

P0 R

Q
2

Measurement error covariance:

PM noise

Define a matrix of random numbers with mean 0 and = anoise: Define a matrix of random numbers with mean 0 and = PMnoise:

randn process

rnorm n

1 , 0 , a noise 1 , 0 , PM noise

randn measurement

rnorm n

http://www.innovatia.com/software/papers/kalman.htm

Page 1 of 2

t Simulate the process: xj

1

a. xj

randn process . 2
j

t Simulate the measurement: zj c. xj randn measurement 0

0

Calculate the estimated position:

x hat

x hat P0

for j 0 .. n Covariance of innovation: Gain matrix: Innovation: Covariance of prediction error: Estimated position: sj Kj Ij Pj zj
1 j

c. Pj. c

R
T

a. Pj. c

. s j
j

c. a. x hat a. Pj. a
1 T

Q
j

Kj. c. Pj. a Kj. Ij

x hat x hat

a. x hat

State estimate:
10

0 10

10

15

20

25

30

20

30

40

50

60 Time (seconds)

True Position Measured Pos Estimated Pos

http://www.innovatia.com/software/papers/kalman.htm

Page 2 of 2