{"action":"created","comment":{"url":"https://api.github.com/repos/harvard-cs205/cs205-homework/pulls/comments/39933601","id":39933601,"diff_hunk":"@@ -30,47 +30,97 @@\n     # Load true trajectory and plot it\n     # Normally, this data wouldn't be available in the real world\n     #####################\n-\n-    # ax.plot(x_coords, y_coords, z_coords,\n-    #         '--b', label='True trajectory')\n+    s_true=np.loadtxt('P4_trajectory.txt',delimiter=',')\n+    #Get transpose\n+    s_true=s_true.transpose()\n+    ax.plot(s_true[0], s_true[1], s_true[2],'--b', label='True trajectory')\n \n     #####################\n     # Part 2:\n     #\n     # Read the observation array and plot it (Part 2)\n     #####################\n-\n-    # ax.plot(x_coords, y_coords, z_coords,\n-    #         '.g', label='Observed trajectory')\n+    m_k=np.loadtxt('P4_measurements.txt',delimiter=',')\n+    #Set up Matrix C\n+    C=np.identity(3)\n+    C[0][0]=1/rx\n+    C[1][1]=1/ry\n+    C[2][2]=1/rz\n+    Col_length=len(m_k[:,0])\n+    x_k=np.zeros([Col_length,3])\n+    i=0\n+    while i<Col_length:\n+     x_k[i]=np.dot(m_k[i],C)\n+     i=i+1\n+    x_k=np.transpose(x_k)\n+    ax.plot(x_k[0], x_k[1], x_k[2], '.g', label='Observed trajectory')\n \n     #####################\n     # Part 3:\n     # Use the initial conditions and propagation matrix for prediction\n     #####################\n \n-    # A = ?\n-    # a = ?\n-    # s = ?\n-\n+    A = np.zeros([6,6])\n+    A[0][0]=A[1][1]=A[2][2]=1\n+    A[0][3]=A[1][4]=A[2][5]=dt\n+    A[3][3]=A[4][4]=A[5][5]=1-c*dt\n+    \n+    a = [0,0,0,0,0,g*dt]\n+ \n+    s = np.array([0,0,2,15,3.5,4.0])\n     # Initial conditions for s0\n+    S=np.zeros([6,K+1])\n+    S[:,0]=s\n     # Compute the rest of sk using Eq (1)\n-\n-    # ax.plot(x_coords, y_coords, z_coords,\n-    #         '-k', label='Blind trajectory')\n+    k=0\n+    while k<K:\n+      S[:,k+1]=np.dot(A,S[:,k])+a\n+      k=k+1\n+    \n+    ax.plot(S[0], S[1], S[2],'-k', label='Blind trajectory')\n \n     #####################\n     # Part 4:\n     # Use the Kalman filter for prediction\n     #####################\n \n-    # B = ?\n-    # C = ?\n+    B = np.zeros([6,6])\n+    B[0][0]=bx;B[1][1]=by;B[2][2]=bz\n+    B[3][3]=bvx;B[4][4]=bvy;B[5][5]=bvz\n+    \n+    C=np.zeros([3,6])\n+    C[0][0]=rx\n+    C[1][1]=ry\n+    C[2][2]=rz\n \n     # Initial conditions for s0 and Sigma0\n     # Compute the rest of sk using Eqs (2), (3), (4), and (5)\n-\n-    # ax.plot(x_coords, y_coords, z_coords,\n-    #         '-r', label='Filtered trajectory')\n+    # predictS\n+    def predictS(s,A,a):\n+      return np.dot(A,s)+a\n+    #predictSig\n+    def predictSig(A,Sig,B):\n+      return np.linalg.inv(A.dot(np.dot(Sig,A.transpose()))+np.dot(B,B.transpose()))\n+    #updateSig\n+    def updateSig(Sig_w,C):\n+      return np.linalg.inv(Sig_w+np.dot(C.transpose(),C))\n+    #updateS\n+    def updateS(Sig,s,C,m):\n+      return np.dot(np.linalg.inv(Sig+np.dot(C.transpose(),C)),(np.dot(Sig,s)+np.dot(C.transpose(),m)))\n+    #Init and Compute\n+    m=np.loadtxt('P4_measurements.txt',delimiter=',')\n+    S=np.zeros([6,K+1])\n+    s=S[:,0]=[0,0,2,15,3.5,4.0]\n+    Sig=0.01*np.identity(6)\n+    k=0\n+    while k<K:\n+     #s=predictS(s,A,a)\n+     s=updateS(predictSig(A,Sig,B),predictS(s,A,a),C,m[k,:])\n+     Sig=updateSig(predictSig(A,Sig,B),C)\n+     S[:,k+1]=s\n+     k=k+1\n+    ax.plot(S[0], S[1], S[2],\n+             '-r', label='Filtered trajectory')\n \n     # Show the plot\n     ax.legend()","path":"HW0/P4/P4.py","position":115,"original_position":115,"commit_id":"2c089b44f2d588e4e22b60e058db04aa88377011","original_commit_id":"2c089b44f2d588e4e22b60e058db04aa88377011","user":{"login":"virgodi","id":14116972,"avatar_url":"https://avatars.githubusercontent.com/u/14116972?v=3","gravatar_id":"","url":"https://api.github.com/users/virgodi","html_url":"https://github.com/virgodi","followers_url":"https://api.github.com/users/virgodi/followers","following_url":"https://api.github.com/users/virgodi/following{/other_user}","gists_url":"https://api.github.com/users/virgodi/gists{/gist_id}","starred_url":"https://api.github.com/users/virgodi/starred{/owner}{/repo}","subscriptions_url":"https://api.github.com/users/virgodi/subscriptions","organizations_url":"https://api.github.com/users/virgodi/orgs","repos_url":"https://api.github.com/users/virgodi/repos","events_url":"https://api.github.com/users/virgodi/events{/privacy}","received_events_url":"https://api.github.com/users/virgodi/received_events","type":"User","site_admin":false},"body":"Your code is very readable ! Well done !","created_at":"2015-09-20T19:46:45Z","updated_at":"2015-09-20T19:46:45Z","html_url":"https://github.com/harvard-cs205/cs205-homework/pull/99#discussion_r39933601","pull_request_url":"https://api.github.com/repos/harvard-cs205/cs205-homework/pulls/99","_links":{"self":{"href":"https://api.github.com/repos/harvard-cs205/cs205-homework/pulls/comments/39933601"},"html":{"href":"https://github.com/harvard-cs205/cs205-homework/pull/99#discussion_r39933601"},"pull_request":{"href":"https://api.github.com/repos/harvard-cs205/cs205-homework/pulls/99"}}},"pull_request":{"url":"https://api.github.com/repos/harvard-cs205/cs205-homework/pulls/99","id":44499290,"html_url":"https://github.com/harvard-cs205/cs205-homework/pull/99","diff_url":"https://github.com/harvard-cs205/cs205-homework/pull/99.diff","patch_url":"https://github.com/harvard-cs205/cs205-homework/pull/99.patch","issue_url":"https://api.github.com/repos/harvard-cs205/cs205-homework/issues/99","number":99,"state":"open","locked":false,"title":"HW0b","user":{"login":"Peilin-D","id":14099137,"avatar_url":"https://avatars.githubusercontent.com/u/14099137?v=3","gravatar_id":"","url":"https://api.github.com/users/Peilin-D","html_url":"https://github.com/Peilin-D","followers_url":"https://api.github.com/users/Peilin-D/followers","following_url":"https://api.github.com/users/Peilin-D/following{/other_user}","gists_url":"https://api.github.com/users/Peilin-D/gists{/gist_id}","starred_url":"https://api.github.com/users/Peilin-D/starred{/owner}{/repo}","subscriptions_url":"https://api.github.com/users/Peilin-D/subscriptions","organizations_url":"https://api.github.com/users/Peilin-D/orgs","repos_url":"https://api.github.com/users/Peilin-D/repos","events_url":"https://api.github.com/users/Peilin-D/events{/privacy}","received_events_url":"https://api.github.com/users/Peilin-D/received_events","type":"User","site_admin":false},"body":"First pull request on Sep 8th","created_at":"2015-09-09T00:38:49Z","updated_at":"2015-09-20T19:46:45Z","closed_at":null,"merged_at":null,"merge_commit_sha":"e07938a04bf3b2156c8d3b61083bfa1db4226ffe","assignee":null,"milestone":null,"commits_url":"https://api.github.com/repos/harvard-cs205/cs205-homework/pulls/99/commits","review_comments_url":"https://api.github.com/repos/harvard-cs205/cs205-homework/pulls/99/comments","review_comment_url":"https://api.github.com/repos/harvard-cs205/cs205-homework/pulls/comments{/number}","comments_url":"https://api.github.com/repos/harvard-cs205/cs205-homework/issues/99/comments","statuses_url":"https://api.github.com/repos/harvard-cs205/cs205-homework/statuses/2c089b44f2d588e4e22b60e058db04aa88377011","head":{"label":"Peilin-D:HW0b","ref":"HW0b","sha":"2c089b44f2d588e4e22b60e058db04aa88377011","user":{"login":"Peilin-D","id":14099137,"avatar_url":"https://avatars.githubusercontent.com/u/14099137?v=3","gravatar_id":"","url":"https://api.github.com/users/Peilin-D","html_url":"https://github.com/Peilin-D","followers_url":"https://api.github.com/users/Peilin-D/followers","following_url":"https://api.github.com/users/Peilin-D/following{/other_user}","gists_url":"https://api.github.com/users/Peilin-D/gists{/gist_id}","starred_url":"https://api.github.com/users/Peilin-D/starred{/owner}{/repo}","subscriptions_url":"https://api.github.com/users/Peilin-D/subscriptions","organizations_url":"https://api.github.com/users/Peilin-D/orgs","repos_url":"https://api.github.com/users/Peilin-D/repos","events_url":"https://api.github.com/users/Peilin-D/events{/privacy}","received_events_url":"https://api.github.com/users/Peilin-D/received_events","type":"User","site_admin":false},"repo":{"id":41919065,"name":"cs205-homework","full_name":"Peilin-D/cs205-homework","owner":{"login":"Peilin-D","id":14099137,"avatar_url":"https://avatars.githubusercontent.com/u/14099137?v=3","gravatar_id":"","url":"https://api.github.com/users/Peilin-D","html_url":"https://github.com/Peilin-D","followers_url":"https://api.github.com/users/Peilin-D/followers","following_url":"https://api.github.com/users/Peilin-D/following{/other_user}","gists_url":"https://api.github.com/users/Peilin-D/gists{/gist_id}","starred_url":"https://api.github.com/users/Peilin-D/starred{/owner}{/repo}","subscriptions_url":"https://api.github.com/users/Peilin-D/subscriptions","organizations_url":"https://api.github.com/users/Peilin-D/orgs","repos_url":"https://api.github.com/users/Peilin-D/repos","events_url":"https://api.github.com/users/Peilin-D/events{/privacy}","received_events_url":"https://api.github.com/users/Peilin-D/received_events","type":"User","site_admin":false},"private":false,"html_url":"https://github.com/Peilin-D/cs205-homework","description":"","fork":true,"url":"https://api.github.com/repos/Peilin-D/cs205-homework","forks_url":"https://api.github.com/repos/Peilin-D/cs205-homework/forks","keys_url":"https://api.github.com/repos/Peilin-D/cs205-homework/keys{/key_id}","collaborators_url":"https://api.github.com/repos/Peilin-D/cs205-homework/collaborators{/collaborator}","teams_url":"https://api.github.com/repos/Peilin-D/cs205-homework/teams","hooks_url":"https://api.github.com/repos/Peilin-D/cs205-homework/hooks","issue_events_url":"https://api.github.com/repos/Peilin-D/cs205-homework/issues/events{/number}","events_url":"https://api.github.com/repos/Peilin-D/cs205-homework/events","assignees_url":"https://api.github.com/repos/Peilin-D/cs205-homework/assignees{/user}","branches_url":"https://api.github.com/repos/Peilin-D/cs205-homework/branches{/branch}","tags_url":"https://api.github.com/repos/Peilin-D/cs205-homework/tags","blobs_url":"https://api.github.com/repos/Peilin-D/cs205-homework/git/blobs{/sha}","git_tags_url":"https://api.github.com/repos/Peilin-D/cs205-homework/git/tags{/sha}","git_refs_url":"https://api.github.com/repos/Peilin-D/cs205-homework/git/refs{/sha}","trees_url":"https://api.github.com/repos/Peilin-D/cs205-homework/git/trees{/sha}","statuses_url":"https://api.github.com/repos/Peilin-D/cs205-homework/statuses/{sha}","languages_url":"https://api.github.com/repos/Peilin-D/cs205-homework/languages","stargazers_url":"https://api.github.com/repos/Peilin-D/cs205-homework/stargazers","contributors_url":"https://api.github.com/repos/Peilin-D/cs205-homework/contributors","subscribers_url":"https://api.github.com/repos/Peilin-D/cs205-homework/subscribers","subscription_url":"https://api.github.com/repos/Peilin-D/cs205-homework/subscription","commits_url":"https://api.github.com/repos/Peilin-D/cs205-homework/commits{/sha}","git_commits_url":"https://api.github.com/repos/Peilin-D/cs205-homework/git/commits{/sha}","comments_url":"https://api.github.com/repos/Peilin-D/cs205-homework/comments{/number}","issue_comment_url":"https://api.github.com/repos/Peilin-D/cs205-homework/issues/comments{/number}","contents_url":"https://api.github.com/repos/Peilin-D/cs205-homework/contents/{+path}","compare_url":"https://api.github.com/repos/Peilin-D/cs205-homework/compare/{base}...{head}","merges_url":"https://api.github.com/repos/Peilin-D/cs205-homework/merges","archive_url":"https://api.github.com/repos/Peilin-D/cs205-homework/{archive_format}{/ref}","downloads_url":"https://api.github.com/repos/Peilin-D/cs205-homework/downloads","issues_url":"https://api.github.com/repos/Peilin-D/cs205-homework/issues{/number}","pulls_url":"https://api.github.com/repos/Peilin-D/cs205-homework/pulls{/number}","milestones_url":"https://api.github.com/repos/Peilin-D/cs205-homework/milestones{/number}","notifications_url":"https://api.github.com/repos/Peilin-D/cs205-homework/notifications{?since,all,participating}","labels_url":"https://api.github.com/repos/Peilin-D/cs205-homework/labels{/name}","releases_url":"https://api.github.com/repos/Peilin-D/cs205-homework/releases{/id}","created_at":"2015-09-04T14:05:21Z","updated_at":"2015-09-04T14:05:21Z","pushed_at":"2015-09-17T03:24:04Z","git_url":"git://github.com/Peilin-D/cs205-homework.git","ssh_url":"git@github.com:Peilin-D/cs205-homework.git","clone_url":"https://github.com/Peilin-D/cs205-homework.git","svn_url":"https://github.com/Peilin-D/cs205-homework","homepage":null,"size":197,"stargazers_count":0,"watchers_count":0,"language":"Python","has_issues":false,"has_downloads":true,"has_wiki":false,"has_pages":false,"forks_count":0,"mirror_url":null,"open_issues_count":0,"forks":0,"open_issues":0,"watchers":0,"default_branch":"master"}},"base":{"label":"harvard-cs205:master","ref":"master","sha":"c56c8eb364fec71d50312e3780ed6d1191a613ee","user":{"login":"harvard-cs205","id":13456002,"avatar_url":"https://avatars.githubusercontent.com/u/13456002?v=3","gravatar_id":"","url":"https://api.github.com/users/harvard-cs205","html_url":"https://github.com/harvard-cs205","followers_url":"https://api.github.com/users/harvard-cs205/followers","following_url":"https://api.github.com/users/harvard-cs205/following{/other_user}","gists_url":"https://api.github.com/users/harvard-cs205/gists{/gist_id}","starred_url":"https://api.github.com/users/harvard-cs205/starred{/owner}{/repo}","subscriptions_url":"https://api.github.com/users/harvard-cs205/subscriptions","organizations_url":"https://api.github.com/users/harvard-cs205/orgs","repos_url":"https://api.github.com/users/harvard-cs205/repos","events_url":"https://api.github.com/users/harvard-cs205/events{/privacy}","received_events_url":"https://api.github.com/users/harvard-cs205/received_events","type":"Organization","site_admin":false},"repo":{"id":40614104,"name":"cs205-homework","full_name":"harvard-cs205/cs205-homework","owner":{"login":"harvard-cs205","id":13456002,"avatar_url":"https://avatars.githubusercontent.com/u/13456002?v=3","gravatar_id":"","url":"https://api.github.com/users/harvard-cs205","html_url":"https://github.com/harvard-cs205","followers_url":"https://api.github.com/users/harvard-cs205/followers","following_url":"https://api.github.com/users/harvard-cs205/following{/other_user}","gists_url":"https://api.github.com/users/harvard-cs205/gists{/gist_id}","starred_url":"https://api.github.com/users/harvard-cs205/starred{/owner}{/repo}","subscriptions_url":"https://api.github.com/users/harvard-cs205/subscriptions","organizations_url":"https://api.github.com/users/harvard-cs205/orgs","repos_url":"https://api.github.com/users/harvard-cs205/repos","events_url":"https://api.github.com/users/harvard-cs205/events{/privacy}","received_events_url":"https://api.github.com/users/harvard-cs205/received_events","type":"Organization","site_admin":false},"private":false,"html_url":"https://github.com/harvard-cs205/cs205-homework","description":"","fork":false,"url":"https://api.github.com/repos/harvard-cs205/cs205-homework","forks_url":"https://api.github.com/repos/harvard-cs205/cs205-homework/forks","keys_url":"https://api.github.com/repos/harvard-cs205/cs205-homework/keys{/key_id}","collaborators_url":"https://api.github.com/repos/harvard-cs205/cs205-homework/collaborators{/collaborator}","teams_url":"https://api.github.com/repos/harvard-cs205/cs205-homework/teams","hooks_url":"https://api.github.com/repos/harvard-cs205/cs205-homework/hooks","issue_events_url":"https://api.github.com/repos/harvard-cs205/cs205-homework/issues/events{/number}","events_url":"https://api.github.com/repos/harvard-cs205/cs205-homework/events","assignees_url":"https://api.github.com/repos/harvard-cs205/cs205-homework/assignees{/user}","branches_url":"https://api.github.com/repos/harvard-cs205/cs205-homework/branches{/branch}","tags_url":"https://api.github.com/repos/harvard-cs205/cs205-homework/tags","blobs_url":"https://api.github.com/repos/harvard-cs205/cs205-homework/git/blobs{/sha}","git_tags_url":"https://api.github.com/repos/harvard-cs205/cs205-homework/git/tags{/sha}","git_refs_url":"https://api.github.com/repos/harvard-cs205/cs205-homework/git/refs{/sha}","trees_url":"https://api.github.com/repos/harvard-cs205/cs205-homework/git/trees{/sha}","statuses_url":"https://api.github.com/repos/harvard-cs205/cs205-homework/statuses/{sha}","languages_url":"https://api.github.com/repos/harvard-cs205/cs205-homework/languages","stargazers_url":"https://api.github.com/repos/harvard-cs205/cs205-homework/stargazers","contributors_url":"https://api.github.com/repos/harvard-cs205/cs205-homework/contributors","subscribers_url":"https://api.github.com/repos/harvard-cs205/cs205-homework/subscribers","subscription_url":"https://api.github.com/repos/harvard-cs205/cs205-homework/subscription","commits_url":"https://api.github.com/repos/harvard-cs205/cs205-homework/commits{/sha}","git_commits_url":"https://api.github.com/repos/harvard-cs205/cs205-homework/git/commits{/sha}","comments_url":"https://api.github.com/repos/harvard-cs205/cs205-homework/comments{/number}","issue_comment_url":"https://api.github.com/repos/harvard-cs205/cs205-homework/issues/comments{/number}","contents_url":"https://api.github.com/repos/harvard-cs205/cs205-homework/contents/{+path}","compare_url":"https://api.github.com/repos/harvard-cs205/cs205-homework/compare/{base}...{head}","merges_url":"https://api.github.com/repos/harvard-cs205/cs205-homework/merges","archive_url":"https://api.github.com/repos/harvard-cs205/cs205-homework/{archive_format}{/ref}","downloads_url":"https://api.github.com/repos/harvard-cs205/cs205-homework/downloads","issues_url":"https://api.github.com/repos/harvard-cs205/cs205-homework/issues{/number}","pulls_url":"https://api.github.com/repos/harvard-cs205/cs205-homework/pulls{/number}","milestones_url":"https://api.github.com/repos/harvard-cs205/cs205-homework/milestones{/number}","notifications_url":"https://api.github.com/repos/harvard-cs205/cs205-homework/notifications{?since,all,participating}","labels_url":"https://api.github.com/repos/harvard-cs205/cs205-homework/labels{/name}","releases_url":"https://api.github.com/repos/harvard-cs205/cs205-homework/releases{/id}","created_at":"2015-08-12T17:18:15Z","updated_at":"2015-09-17T01:30:31Z","pushed_at":"2015-09-19T14:49:47Z","git_url":"git://github.com/harvard-cs205/cs205-homework.git","ssh_url":"git@github.com:harvard-cs205/cs205-homework.git","clone_url":"https://github.com/harvard-cs205/cs205-homework.git","svn_url":"https://github.com/harvard-cs205/cs205-homework","homepage":null,"size":16490,"stargazers_count":4,"watchers_count":4,"language":"Python","has_issues":true,"has_downloads":true,"has_wiki":false,"has_pages":false,"forks_count":99,"mirror_url":null,"open_issues_count":184,"forks":99,"open_issues":184,"watchers":4,"default_branch":"master"}},"_links":{"self":{"href":"https://api.github.com/repos/harvard-cs205/cs205-homework/pulls/99"},"html":{"href":"https://github.com/harvard-cs205/cs205-homework/pull/99"},"issue":{"href":"https://api.github.com/repos/harvard-cs205/cs205-homework/issues/99"},"comments":{"href":"https://api.github.com/repos/harvard-cs205/cs205-homework/issues/99/comments"},"review_comments":{"href":"https://api.github.com/repos/harvard-cs205/cs205-homework/pulls/99/comments"},"review_comment":{"href":"https://api.github.com/repos/harvard-cs205/cs205-homework/pulls/comments{/number}"},"commits":{"href":"https://api.github.com/repos/harvard-cs205/cs205-homework/pulls/99/commits"},"statuses":{"href":"https://api.github.com/repos/harvard-cs205/cs205-homework/statuses/2c089b44f2d588e4e22b60e058db04aa88377011"}}}}