2

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

Community
  • 1
Haider
  • 97
  • 1
  • 9
  • 1
    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
  • 1
    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
  • @PeterK. Could you please mention 3D example in Kalman Filter.I know basics of KF but would like to see how it works for 3D case.Please if you know any example would be nice of you. – Haider Sep 29 '15 at 13:07
  • 1
    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
  • @PeterK.Thansk for sharing the information.I will go through it. – Haider Sep 29 '15 at 13:16
  • @PeterK.Thanks for posting the link.It contains so much useful information regarding the derivation of KF equations. Kindly would so nice if you post a 3D example of Matlab implementation of KF please. I see some examples available on internet but are so complicated .Could you post a Matlab example please. Thanks in advance! Sorry for any inconvenience! – Haider Sep 29 '15 at 13:25
  • 1
    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
  • @PeterK. Thanks for posting the 1D example.I will go through it and see what happens. Thanks again – Haider Sep 29 '15 at 13:42
  • @PeterK. I have just gone through this 1D example of the given code.In that code in last for loop they have used a function called kalm() .But when I run this code in Matlab it says " Undefined function or method 'kalm' for input arguments of type 'double'." Is this function not defined in Matlab or I am making some mistake while running.Please would be so nice if you clarify it kindly.Thanks! – Haider Sep 29 '15 at 13:58
  • http://forge.scilab.org/index.php/p/sCiMake/source/tree/885da23a2652da788a995392149af0403c3fde40/scilab-5.2.0/modules/signal_processing/macros/kalm.sci – Peter K. Sep 29 '15 at 14:08
  • Is there more that needs to be answered? – Peter K. Nov 04 '15 at 14:48
  • @PeterK. I am working on it if I have some thing to ask will write over here.Thanks for your notice – Haider Nov 05 '15 at 13:29
  • Was there anything else needed here? I'm just trying to close out questions with no accepted answer. If you need more info, please let us know. – Peter K. Dec 04 '15 at 15:54

1 Answers1

2

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

Peter K.
  • 25,714
  • 9
  • 46
  • 91
  • Thanks for your comment I will do that and see.Thanks again! – Haider 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 – Haider Sep 30 '15 at 16:20
  • 1
    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 – Haider Oct 05 '15 at 13:01
  • On which criteria Q and R should be chosen (values and dimensions) in 3D case.Please respond thanks. – Haider Oct 05 '15 at 13:19
  • Are you just measuring position for $x$, $y$, and $z$? Then the signal model above should work. $\mathbf{z}_k$ are your measurements. If your model has no velocity, then the position won't change. If your model has not acceleration input, then the velocity won't change and so the position won't change. – Peter K. Oct 05 '15 at 14:16
  • Peter K:Thanks for the reply.Yes.I am just measuring position for x,y and z.You mean this model should work without velocity and acceleration?Then what should be xk and F,G,H,Q and R.Please respond.I am sorry if I say some thing silly – Haider Oct 05 '15 at 14:31
  • The model assumes that only position is being measured. For the model to change the position, it needs to have a mechanism to do that: velocity and acceleration. Otherwise the position will not change. The matrices above should work for the KF with your case. – Peter K. Oct 05 '15 at 15:32
  • Peter K thanks for your comment.In your comment kindly can you elaborate little bit more this line please:"For the model to change the position, it needs to have a mechanism to do that: velocity and acceleration". – Haider Oct 05 '15 at 16:07
  • @Haider : Does your object change its position? If it does, then it has to have a velocity (change of position over time). – Peter K. Oct 05 '15 at 16:12
  • No my object does not change its position .What should I use in xk and other matrics F,G,H,Q and R please help. – Haider Oct 05 '15 at 16:17
  • The you don't need a Kalman filter. Just keep averaging the position measurements you get. – Peter K. Oct 05 '15 at 16:20
  • Ok.Now I get your point.So if I want to use KF then it assumes the object changes its position and so needs velocity.This model only works if the object changes its position.Thanks Peter K .I will try it out and see and let you know – Haider Oct 05 '15 at 16:22