Extended Kalman Filter measurement residual computation

Multi tool use
Multi tool use











up vote
0
down vote

favorite












I am trying to follow the computation of EKF presented in this paper http://citeseerx.ist.psu.edu/viewdoc/download;jsessionid=C9CB210A45F0D7ED5CA7DE174F1A5490?doi=10.1.1.681.8390&rep=rep1&type=pdf (p.16-18), with the only difference that my system state does not include the acceleration, so it is a $4x1$ matrix:



$$x_x(t) = begin{bmatrix}
x(t) \
v_x(t) \
y(t) \
v_y(t)
end{bmatrix} $$

where $x(t)$ and $y(t)$ are the cartesian coordinates and $v_x(t)$ and $v_y(t)$ are the velocity components.



I have problem computing the State Estimate Update step (eq. 17 in the paper), where the measurement residual needs to be calculated. According to the paper, the State Estimate Update is:



$$hat{x}_x(k^{+}) = hat{x}_x(k^{-}) + K(k)[z_{z}(k)-h(hat{x}_x(k^{-}))]$$



where
$$z_z(k) = begin{bmatrix}
z_x(k) \
z_y(k)
end{bmatrix},
$$

where $z_x(k)$ and $z_y(k)$ are the measurements of $x$ and $y$ positions and
$$
h(x_x(k)) = begin{bmatrix}
x(k) &0 &0 &0 \
0 &0 &y(k) &0
end{bmatrix}. $$



$K(k)$ is the Kalman Gain, which in my case is a $4x2$ matrix.



My question is, how do I subtract $h(hat{x}_x(k^{-}))$ which is a $2x4$ matrix from $z_z(k)$, which is a $2x1$ matrix?
If I am not mistaken, the result of $K(k)[z_{z}(k)-h(hat{x}_x(k^{-}))]$ needs to be a $4x1$ matrix so that I can later add it to the previous estimate $hat{x}_x(k^{-})$, which is also $4x1$.










share|cite|improve this question


























    up vote
    0
    down vote

    favorite












    I am trying to follow the computation of EKF presented in this paper http://citeseerx.ist.psu.edu/viewdoc/download;jsessionid=C9CB210A45F0D7ED5CA7DE174F1A5490?doi=10.1.1.681.8390&rep=rep1&type=pdf (p.16-18), with the only difference that my system state does not include the acceleration, so it is a $4x1$ matrix:



    $$x_x(t) = begin{bmatrix}
    x(t) \
    v_x(t) \
    y(t) \
    v_y(t)
    end{bmatrix} $$

    where $x(t)$ and $y(t)$ are the cartesian coordinates and $v_x(t)$ and $v_y(t)$ are the velocity components.



    I have problem computing the State Estimate Update step (eq. 17 in the paper), where the measurement residual needs to be calculated. According to the paper, the State Estimate Update is:



    $$hat{x}_x(k^{+}) = hat{x}_x(k^{-}) + K(k)[z_{z}(k)-h(hat{x}_x(k^{-}))]$$



    where
    $$z_z(k) = begin{bmatrix}
    z_x(k) \
    z_y(k)
    end{bmatrix},
    $$

    where $z_x(k)$ and $z_y(k)$ are the measurements of $x$ and $y$ positions and
    $$
    h(x_x(k)) = begin{bmatrix}
    x(k) &0 &0 &0 \
    0 &0 &y(k) &0
    end{bmatrix}. $$



    $K(k)$ is the Kalman Gain, which in my case is a $4x2$ matrix.



    My question is, how do I subtract $h(hat{x}_x(k^{-}))$ which is a $2x4$ matrix from $z_z(k)$, which is a $2x1$ matrix?
    If I am not mistaken, the result of $K(k)[z_{z}(k)-h(hat{x}_x(k^{-}))]$ needs to be a $4x1$ matrix so that I can later add it to the previous estimate $hat{x}_x(k^{-})$, which is also $4x1$.










    share|cite|improve this question
























      up vote
      0
      down vote

      favorite









      up vote
      0
      down vote

      favorite











      I am trying to follow the computation of EKF presented in this paper http://citeseerx.ist.psu.edu/viewdoc/download;jsessionid=C9CB210A45F0D7ED5CA7DE174F1A5490?doi=10.1.1.681.8390&rep=rep1&type=pdf (p.16-18), with the only difference that my system state does not include the acceleration, so it is a $4x1$ matrix:



      $$x_x(t) = begin{bmatrix}
      x(t) \
      v_x(t) \
      y(t) \
      v_y(t)
      end{bmatrix} $$

      where $x(t)$ and $y(t)$ are the cartesian coordinates and $v_x(t)$ and $v_y(t)$ are the velocity components.



      I have problem computing the State Estimate Update step (eq. 17 in the paper), where the measurement residual needs to be calculated. According to the paper, the State Estimate Update is:



      $$hat{x}_x(k^{+}) = hat{x}_x(k^{-}) + K(k)[z_{z}(k)-h(hat{x}_x(k^{-}))]$$



      where
      $$z_z(k) = begin{bmatrix}
      z_x(k) \
      z_y(k)
      end{bmatrix},
      $$

      where $z_x(k)$ and $z_y(k)$ are the measurements of $x$ and $y$ positions and
      $$
      h(x_x(k)) = begin{bmatrix}
      x(k) &0 &0 &0 \
      0 &0 &y(k) &0
      end{bmatrix}. $$



      $K(k)$ is the Kalman Gain, which in my case is a $4x2$ matrix.



      My question is, how do I subtract $h(hat{x}_x(k^{-}))$ which is a $2x4$ matrix from $z_z(k)$, which is a $2x1$ matrix?
      If I am not mistaken, the result of $K(k)[z_{z}(k)-h(hat{x}_x(k^{-}))]$ needs to be a $4x1$ matrix so that I can later add it to the previous estimate $hat{x}_x(k^{-})$, which is also $4x1$.










      share|cite|improve this question













      I am trying to follow the computation of EKF presented in this paper http://citeseerx.ist.psu.edu/viewdoc/download;jsessionid=C9CB210A45F0D7ED5CA7DE174F1A5490?doi=10.1.1.681.8390&rep=rep1&type=pdf (p.16-18), with the only difference that my system state does not include the acceleration, so it is a $4x1$ matrix:



      $$x_x(t) = begin{bmatrix}
      x(t) \
      v_x(t) \
      y(t) \
      v_y(t)
      end{bmatrix} $$

      where $x(t)$ and $y(t)$ are the cartesian coordinates and $v_x(t)$ and $v_y(t)$ are the velocity components.



      I have problem computing the State Estimate Update step (eq. 17 in the paper), where the measurement residual needs to be calculated. According to the paper, the State Estimate Update is:



      $$hat{x}_x(k^{+}) = hat{x}_x(k^{-}) + K(k)[z_{z}(k)-h(hat{x}_x(k^{-}))]$$



      where
      $$z_z(k) = begin{bmatrix}
      z_x(k) \
      z_y(k)
      end{bmatrix},
      $$

      where $z_x(k)$ and $z_y(k)$ are the measurements of $x$ and $y$ positions and
      $$
      h(x_x(k)) = begin{bmatrix}
      x(k) &0 &0 &0 \
      0 &0 &y(k) &0
      end{bmatrix}. $$



      $K(k)$ is the Kalman Gain, which in my case is a $4x2$ matrix.



      My question is, how do I subtract $h(hat{x}_x(k^{-}))$ which is a $2x4$ matrix from $z_z(k)$, which is a $2x1$ matrix?
      If I am not mistaken, the result of $K(k)[z_{z}(k)-h(hat{x}_x(k^{-}))]$ needs to be a $4x1$ matrix so that I can later add it to the previous estimate $hat{x}_x(k^{-})$, which is also $4x1$.







      kalman-filter






      share|cite|improve this question













      share|cite|improve this question











      share|cite|improve this question




      share|cite|improve this question










      asked Nov 15 at 11:23









      Anemone

      32




      32






















          1 Answer
          1






          active

          oldest

          votes

















          up vote
          0
          down vote



          accepted










          I suspect it is a mistake and the actual equation should have been



          $$
          h(x(k)) = begin{bmatrix}
          1 & 0 & 0 & 0 & 0 & 0 \
          0 & 0 & 1 & 0 & 0 & 0
          end{bmatrix} x(k).
          $$



          In this case equation $(9)$ would also make more sense. So in your case it would be



          $$
          h(x(k)) = begin{bmatrix}
          1 & 0 & 0 & 0 \
          0 & 0 & 1 & 0
          end{bmatrix} x(k).
          $$






          share|cite|improve this answer





















          • Makes more sense now, thank you!
            – Anemone
            Nov 16 at 11:14










          • @Anemone Also equation $(2)$ should not have a dot on top of $x$, so I would not trust everything immediately from that paper directly if I where you, because it might contain more mistakes.
            – Kwin van der Veen
            Nov 16 at 11:24











          Your Answer





          StackExchange.ifUsing("editor", function () {
          return StackExchange.using("mathjaxEditing", function () {
          StackExchange.MarkdownEditor.creationCallbacks.add(function (editor, postfix) {
          StackExchange.mathjaxEditing.prepareWmdForMathJax(editor, postfix, [["$", "$"], ["\\(","\\)"]]);
          });
          });
          }, "mathjax-editing");

          StackExchange.ready(function() {
          var channelOptions = {
          tags: "".split(" "),
          id: "69"
          };
          initTagRenderer("".split(" "), "".split(" "), channelOptions);

          StackExchange.using("externalEditor", function() {
          // Have to fire editor after snippets, if snippets enabled
          if (StackExchange.settings.snippets.snippetsEnabled) {
          StackExchange.using("snippets", function() {
          createEditor();
          });
          }
          else {
          createEditor();
          }
          });

          function createEditor() {
          StackExchange.prepareEditor({
          heartbeatType: 'answer',
          convertImagesToLinks: true,
          noModals: true,
          showLowRepImageUploadWarning: true,
          reputationToPostImages: 10,
          bindNavPrevention: true,
          postfix: "",
          imageUploader: {
          brandingHtml: "Powered by u003ca class="icon-imgur-white" href="https://imgur.com/"u003eu003c/au003e",
          contentPolicyHtml: "User contributions licensed under u003ca href="https://creativecommons.org/licenses/by-sa/3.0/"u003ecc by-sa 3.0 with attribution requiredu003c/au003e u003ca href="https://stackoverflow.com/legal/content-policy"u003e(content policy)u003c/au003e",
          allowUrls: true
          },
          noCode: true, onDemand: true,
          discardSelector: ".discard-answer"
          ,immediatelyShowMarkdownHelp:true
          });


          }
          });














          draft saved

          draft discarded


















          StackExchange.ready(
          function () {
          StackExchange.openid.initPostLogin('.new-post-login', 'https%3a%2f%2fmath.stackexchange.com%2fquestions%2f2999571%2fextended-kalman-filter-measurement-residual-computation%23new-answer', 'question_page');
          }
          );

          Post as a guest















          Required, but never shown

























          1 Answer
          1






          active

          oldest

          votes








          1 Answer
          1






          active

          oldest

          votes









          active

          oldest

          votes






          active

          oldest

          votes








          up vote
          0
          down vote



          accepted










          I suspect it is a mistake and the actual equation should have been



          $$
          h(x(k)) = begin{bmatrix}
          1 & 0 & 0 & 0 & 0 & 0 \
          0 & 0 & 1 & 0 & 0 & 0
          end{bmatrix} x(k).
          $$



          In this case equation $(9)$ would also make more sense. So in your case it would be



          $$
          h(x(k)) = begin{bmatrix}
          1 & 0 & 0 & 0 \
          0 & 0 & 1 & 0
          end{bmatrix} x(k).
          $$






          share|cite|improve this answer





















          • Makes more sense now, thank you!
            – Anemone
            Nov 16 at 11:14










          • @Anemone Also equation $(2)$ should not have a dot on top of $x$, so I would not trust everything immediately from that paper directly if I where you, because it might contain more mistakes.
            – Kwin van der Veen
            Nov 16 at 11:24















          up vote
          0
          down vote



          accepted










          I suspect it is a mistake and the actual equation should have been



          $$
          h(x(k)) = begin{bmatrix}
          1 & 0 & 0 & 0 & 0 & 0 \
          0 & 0 & 1 & 0 & 0 & 0
          end{bmatrix} x(k).
          $$



          In this case equation $(9)$ would also make more sense. So in your case it would be



          $$
          h(x(k)) = begin{bmatrix}
          1 & 0 & 0 & 0 \
          0 & 0 & 1 & 0
          end{bmatrix} x(k).
          $$






          share|cite|improve this answer





















          • Makes more sense now, thank you!
            – Anemone
            Nov 16 at 11:14










          • @Anemone Also equation $(2)$ should not have a dot on top of $x$, so I would not trust everything immediately from that paper directly if I where you, because it might contain more mistakes.
            – Kwin van der Veen
            Nov 16 at 11:24













          up vote
          0
          down vote



          accepted







          up vote
          0
          down vote



          accepted






          I suspect it is a mistake and the actual equation should have been



          $$
          h(x(k)) = begin{bmatrix}
          1 & 0 & 0 & 0 & 0 & 0 \
          0 & 0 & 1 & 0 & 0 & 0
          end{bmatrix} x(k).
          $$



          In this case equation $(9)$ would also make more sense. So in your case it would be



          $$
          h(x(k)) = begin{bmatrix}
          1 & 0 & 0 & 0 \
          0 & 0 & 1 & 0
          end{bmatrix} x(k).
          $$






          share|cite|improve this answer












          I suspect it is a mistake and the actual equation should have been



          $$
          h(x(k)) = begin{bmatrix}
          1 & 0 & 0 & 0 & 0 & 0 \
          0 & 0 & 1 & 0 & 0 & 0
          end{bmatrix} x(k).
          $$



          In this case equation $(9)$ would also make more sense. So in your case it would be



          $$
          h(x(k)) = begin{bmatrix}
          1 & 0 & 0 & 0 \
          0 & 0 & 1 & 0
          end{bmatrix} x(k).
          $$







          share|cite|improve this answer












          share|cite|improve this answer



          share|cite|improve this answer










          answered Nov 16 at 9:38









          Kwin van der Veen

          5,1202826




          5,1202826












          • Makes more sense now, thank you!
            – Anemone
            Nov 16 at 11:14










          • @Anemone Also equation $(2)$ should not have a dot on top of $x$, so I would not trust everything immediately from that paper directly if I where you, because it might contain more mistakes.
            – Kwin van der Veen
            Nov 16 at 11:24


















          • Makes more sense now, thank you!
            – Anemone
            Nov 16 at 11:14










          • @Anemone Also equation $(2)$ should not have a dot on top of $x$, so I would not trust everything immediately from that paper directly if I where you, because it might contain more mistakes.
            – Kwin van der Veen
            Nov 16 at 11:24
















          Makes more sense now, thank you!
          – Anemone
          Nov 16 at 11:14




          Makes more sense now, thank you!
          – Anemone
          Nov 16 at 11:14












          @Anemone Also equation $(2)$ should not have a dot on top of $x$, so I would not trust everything immediately from that paper directly if I where you, because it might contain more mistakes.
          – Kwin van der Veen
          Nov 16 at 11:24




          @Anemone Also equation $(2)$ should not have a dot on top of $x$, so I would not trust everything immediately from that paper directly if I where you, because it might contain more mistakes.
          – Kwin van der Veen
          Nov 16 at 11:24


















          draft saved

          draft discarded




















































          Thanks for contributing an answer to Mathematics Stack Exchange!


          • Please be sure to answer the question. Provide details and share your research!

          But avoid



          • Asking for help, clarification, or responding to other answers.

          • Making statements based on opinion; back them up with references or personal experience.


          Use MathJax to format equations. MathJax reference.


          To learn more, see our tips on writing great answers.





          Some of your past answers have not been well-received, and you're in danger of being blocked from answering.


          Please pay close attention to the following guidance:


          • Please be sure to answer the question. Provide details and share your research!

          But avoid



          • Asking for help, clarification, or responding to other answers.

          • Making statements based on opinion; back them up with references or personal experience.


          To learn more, see our tips on writing great answers.




          draft saved


          draft discarded














          StackExchange.ready(
          function () {
          StackExchange.openid.initPostLogin('.new-post-login', 'https%3a%2f%2fmath.stackexchange.com%2fquestions%2f2999571%2fextended-kalman-filter-measurement-residual-computation%23new-answer', 'question_page');
          }
          );

          Post as a guest















          Required, but never shown





















































          Required, but never shown














          Required, but never shown












          Required, but never shown







          Required, but never shown

































          Required, but never shown














          Required, but never shown












          Required, but never shown







          Required, but never shown







          CdLRyZo,5wE1F0C jXHgrK Ge3xFrzRzBoL,IDKQs,t2Tu0b3x 5UW0LAGfDJN09
          Fx nVFtjJu1JPN Kx2XOiH5Bpxwo4hzEvT 1HyLIIG

          Popular posts from this blog

          mysqli_query(): Empty query in /home/lucindabrummitt/public_html/blog/wp-includes/wp-db.php on line 1924

          How to change which sound is reproduced for terminal bell?

          Can I use Tabulator js library in my java Spring + Thymeleaf project?