# Kalman Filter to estimate 3D position of a node

Code given on this link works for 1D: Kalman filter for position and velocity: introducing speed estimates

In my problem I need to estimate 3D position.What is the criteria ?

How F, G ,H,Q and R change in 3D case. This problem is restricted to estimate position only (No velocity,No acceleration).What measurement vector should contain.Only positions? Please respond!I am sorry for any inconvenience

• Why do you think filtering is involved in this question at all? There is no time-dependence on any of the quantities you outline. Note that I have changed some of the notation... you originally had two $r_1$s which confused me. I changed it so that $d_k = | r - r_k|$ etc. – Peter K. Sep 29 '15 at 12:54
• @PeterK.I understand what you mean.So my point is how I can use Kalman filter for 3D case.Is there any example or some explanation please.I am sorry if I said some thing wrong. – Haider Sep 29 '15 at 13:03
• That's the problem: I can't see where filtering comes in here. This is just an arithmetic problem. – Peter K. Sep 29 '15 at 13:03
• Just use Wikipedia but augment the state as $\mathbf{x}_k = [ x\ \dot{x}\ y\ \dot{y}\ z\ \dot{z} ]^T$ – Peter K. Sep 29 '15 at 13:13
• Haider, check out this answer. It just does the 1D case, but 3D just extends this as I indicated above. – Peter K. Sep 29 '15 at 13:30

As I said in the comments, just follow the 1D case from Wikipedia and augment it with the extra $y$ and $z$ dimensions (and velocities): $$\mathbf{x}_k = \left [ \begin{array}{c} x\\ \dot{x}\\ y\\ \dot{y}\\ z\\ \dot{z} \end{array} \right]$$
You will also need to augment $\mathbf{F}$ and $\mathbf{G}$: $$\mathbf{F} = \left[ \begin{array}{cccccc} 1 & \Delta t & 0 & 0 & 0 & 0\\ 0 & 1 & 0 & 0 & 0 & 0\\ 0 & 0 & 1 & \Delta t & 0 & 0\\ 0 & 0 & 0 & 1 & 0 & 0\\ 0 & 0 & 0 & 0 & 1 & \Delta t \\ 0 & 0 & 0 & 0 & 0 & 1 \\ \end{array} \right]$$ $$\mathbf{G} = \left[ \begin{array}{c} \frac{\Delta t^2}{2}\\ \Delta t\\ \frac{\Delta t^2}{2}\\ \Delta t\\ \frac{\Delta t^2}{2}\\ \Delta t\\ \end{array} \right]$$
And then $\mathbf{H}$ is just $$\mathbf{H} = \left[ \begin{array}{cccccc} 1 & 0 & 0 & 0 & 0 & 0\\ 0 & 0 & 1 & 0 & 0 & 0\\ 0 & 0 & 0 & 0 & 1 & 0\\ \end{array} \right]$$ so that $\mathbf{z}_k$ is $$\mathbf{z}_k = \left[ \begin{array}{c} x\\ y\\ z \end{array} \right]$$
• Sorry, that sounds like a debugging issue... Are your $\mathbf{Q}$ and $\mathbf{R}$ matrices singular? Did you set the initial covariance matrix to be non-singular? – Peter K. Sep 30 '15 at 18:18