1
$\begingroup$

I have a simple system with Kalman filter state defined as $[x, y, \dot{x}, \dot{y}]$. Now let $p = [x, y]$ and $v = [\dot{x}, \dot{y}]$, and apply a homography transformation: $$ H=\left[\begin{array}{ll}H_{1} & h_{2} \\ (2 \times 2) & (2 \times 1) \\ h_{3}^{\top} & h_{4} \\ (1 \times 2) & (1 \times 1)\end{array}\right] $$ $$ g\left(p\right)=\frac{H_{1} p+h_{2}}{h_{3}^{\top} p+h_{4}} $$ $$ \begin{aligned} G(p) &=\frac{\partial}{\partial p} g(p) \\ &=\frac{\left(h_{3}^{\top} p+h_{4}\right) H_{1}-\left(H_{1} p+h_{2}\right) h_{3}^{\top}}{\left(h_{3}^{\top} p+h_{4}\right)^{2}} \end{aligned} $$ where $G(p)$ here is a $2\times2$ Jacobian of the transformation with respect to the position vector. To transform the speed and velocity I did something similar to this paper https://scholarsarchive.byu.edu/cgi/viewcontent.cgi?article=1301&context=studentpub $$p^{\prime}=g(p)$$ $$v^{\prime}=G(p)v$$ Now I need to compute the $4\times4$ Jacobian $F$ of the entire state transformation function, i.e. differentiate the above two equations somehow with respect to my state vector, to transform the covariance $C^{\prime} = FCF^{T}$, but I'm not sure how to proceed.

My solution: $$ \frac{\partial s^{\prime}}{\partial s} = \frac{(h_{3}^{T}E_{1}^Ts + h_{4})E_{1}H_{1}E_{1}^{T} + (E_{1}H_{1}E_{1}^{T}s + E_{1}h_{2})h_{3}^{T}E_{1}^T}{(h_{3}^{T}E_{1}^Ts + h_{4})^2} + \frac{(h_{3}^{T}E_{1}^Ts + h_{4})^2\Big(\big((h_{3}^{T}E_{1}^Ts + h_{4})E_{2}H_{1}E_{2}^{T} + E_{2}H_{1}E_{2}^{T}sh_{3}^{T}E_{1}^T\big)-\big((E_{2}H_{1}E_{1}^{T}s + E_{2}H_{2})h_{3}^{T}E_{2}^T + h_{3}^{T}E_{2}^TsE_{2}H_{1}E_{1}^{T}\big)\Big) - \big((h_{3}^{T}E_{1}^Ts + h_{4})E_{2}H_{1}E_{2}^{T}s-(E_{2}H_{1}E_{1}^{T}s+E_{2}H_{2})h_{3}^{T}E_{2}^Ts)\big)2h_{3}^{T}E_{1}^T}{(h_{3}^{T}E_{1}^Ts + h_{4})^4} $$

$\endgroup$

1 Answer 1

0
$\begingroup$

Define the $4\times 1$ state vector $$\eqalign{ s = \pmatrix{p\\v} }$$ and $4\times 2$ matrix analogs $\big\{E_k\big\}$ of the $2\times 1$ cartesian basis vectors $\{e_k\}$ $$\eqalign{ e_1 &= \pmatrix{{\tt1}\\0},\qquad &e_2 &= \pmatrix{0\\{\tt1}},\qquad &I_2&=e_1e_1^T+e_2e_2^T \\ E_1 &= \pmatrix{I_2\\0_2},\qquad &E_2 &= \pmatrix{0_2\\I_2},\qquad &I_4 &= E_1E_1^T+E_2E_2^T \\ I_2 &= E_1^TE_1,\qquad &I_2 &= E_2^TE_2,\qquad &0_2 &= E_1^TE_2 = E_2^TE_1 \\ p &= E_1^Ts,\qquad &v &= E_2^Ts,\qquad &I_4 &= \frac{\partial s}{\partial s} \\ }$$ Write the primed quantities in term of $s$ $$\eqalign{ p' &= E_1^Ts' &= \frac{H_1E_1^Ts+h_2}{h_3^TE_1^Ts+h_4} \\ v' &= E_2^Ts' &= \frac{(h_3^TE_1^Ts+h_4) H_{1}E_2^Ts-(H_1E_1^Ts+h_2) h_3^TE_2^Ts}{(h_3^TE_1^Ts+h_4)^2} }$$ Multiply the first equation by $E_1$ , the second by $E_2$, and add them together to obtain $$\eqalign{ s' &= \frac{E_1H_1E_1^Ts+E_1h_2}{h_3^TE_1^Ts+h_4} + \frac{(h_3^TE_1^Ts+h_4)E_2H_1E_2^Ts-(E_2H_1E_1^Ts+E_2h_2) h_3^TE_2^Ts}{(h_3^TE_1^Ts+h_4)^2} \\ F &= \frac{\partial s'}{\partial s} \;=\; \ldots \\ }$$ I'll leave the tedious details of that last calculation to you.


Update

To keep things tidy define $$\eqalign{ w_{jk} &= E_jh_k \quad &&M_{jk} = E_jH_1E_k^T \qquad M = M_{11} + M_{22} \\ \alpha &= w_{13}^Ts+h_4 &\implies\quad&\frac{\partial\alpha}{\partial s} = w_{13}^T \\ \beta &= w_{23}^Ts &\implies&\frac{\partial\beta}{\partial s} = w_{23}^T \\ }$$ Then calculate
$$\eqalign{ s'&= \alpha^{-1} (Ms+w_{12}) - \alpha^{-2}\beta(M_{21}s+w_{22}) \\ \\ \frac{\partial s'}{\partial s} &= \alpha^{-1} \big(M\big) \\ &-\; \alpha^{-2} \Big( (Ms + w_{12})w_{13}^T + \beta M_{21} + (M_{21}s + w_{22})w_{23}^T\Big) \\ &+\; 2\alpha^{-3}\beta\Big((M_{21}s + w_{22})w_{13}^T\Big) \\ }$$

$\endgroup$
3
  • $\begingroup$ That's exactly what I'm looking for. Thanks for your detailed answer! I just updated the post with my final solution, can you help me check if it's right? Matrix calculus is pretty new to me, I simply applied quotient rule and product rule. My solution seems to pass the 4x4 dimension check. $\endgroup$ Oct 6, 2020 at 5:47
  • $\begingroup$ Let $\alpha=(h_3^TE_1^Ts+h_4),\,$ then each term in $F$ must include factors of at least $\alpha^{-1}$ and at most $\alpha^{-3}$. But in your solution I see some terms without any $\alpha$ factor, and some with $\alpha^{-4},\,$ so those cannot be right. $\endgroup$
    – greg
    Oct 6, 2020 at 12:36
  • $\begingroup$ @AlexisYang I updated the answer with my own calculation of $F$. $\endgroup$
    – greg
    Oct 6, 2020 at 20:47

You must log in to answer this question.

Not the answer you're looking for? Browse other questions tagged .