# 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. 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]$$

• Thanks for your comment I will do that and see.Thanks again! Sep 30 '15 at 13:04
• Thank you so much for your help.I have just one problem .I have run the code for 1D in Matlab but it gives me NAN result. I dont know why thins error exist .Would be so nice if you help please.Thanks Sep 30 '15 at 16:20
• 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
• Peter K:Thanks for your help.I am trying to run this code for 3D position.You said I need velocities in my xk vector (see above your comment).In my case I am just dealing with position (No velocity NO acceleration).Do I still need to add velocities .In my case I do not have information about velocity (I only have measured 3D position).And If I do not add velocities how does it effect the dimensions of F, G and H. Please help..Thanks Oct 5 '15 at 13:01
• On which criteria Q and R should be chosen (values and dimensions) in 3D case.Please respond thanks. Oct 5 '15 at 13:19