0
$\begingroup$

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

$\endgroup$
  • 1
    $\begingroup$ 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. $\endgroup$ – Peter K. Sep 29 '15 at 12:54
  • $\begingroup$ @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. $\endgroup$ – Haider Sep 29 '15 at 13:03
  • 1
    $\begingroup$ That's the problem: I can't see where filtering comes in here. This is just an arithmetic problem. $\endgroup$ – Peter K. Sep 29 '15 at 13:03
  • 1
    $\begingroup$ Just use Wikipedia but augment the state as $\mathbf{x}_k = [ x\ \dot{x}\ y\ \dot{y}\ z\ \dot{z} ]^T$ $\endgroup$ – Peter K. Sep 29 '15 at 13:13
  • 1
    $\begingroup$ Haider, check out this answer. It just does the 1D case, but 3D just extends this as I indicated above. $\endgroup$ – Peter K. Sep 29 '15 at 13:30
1
$\begingroup$

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

$\endgroup$
  • $\begingroup$ Thanks for your comment I will do that and see.Thanks again! $\endgroup$ – Haider Sep 30 '15 at 13:04
  • $\begingroup$ 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 $\endgroup$ – Haider Sep 30 '15 at 16:20
  • 1
    $\begingroup$ 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? $\endgroup$ – Peter K. Sep 30 '15 at 18:18
  • $\begingroup$ 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 $\endgroup$ – Haider Oct 5 '15 at 13:01
  • $\begingroup$ On which criteria Q and R should be chosen (values and dimensions) in 3D case.Please respond thanks. $\endgroup$ – Haider Oct 5 '15 at 13:19

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service, privacy policy and cookie policy

Not the answer you're looking for? Browse other questions tagged or ask your own question.